From eb6c3cc42deb1358f748a31054dc293e2faba98a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 24 Apr 2024 00:20:12 +0200 Subject: [PATCH] Joint limits resource manager integration --- hardware_interface/src/resource_manager.cpp | 153 +++++++ .../joint_limits/joint_soft_limiter.hpp | 70 +++ joint_limits/src/soft_joint_limiter.cpp | 413 ++++++++---------- 3 files changed, 409 insertions(+), 227 deletions(-) create mode 100644 joint_limits/include/joint_limits/joint_soft_limiter.hpp diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index d6827497b5..568d67caf1 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -34,6 +34,10 @@ #include "hardware_interface/system.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limiter_struct.hpp" +#include "joint_limits/joint_saturation_limiter.hpp" +#include "joint_limits/joint_soft_limiter.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "pluginlib/class_loader.hpp" #include "rcutils/logging_macros.h" @@ -662,6 +666,130 @@ class ResourceStorage hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); } + void import_joint_limiters(const std::vector & hardware_infos) + { + for (const auto & hw_info : hardware_infos) + { + limiters_data_[hw_info.name] = {}; + for (const auto & [joint_name, limits] : hw_info.limits) + { + std::unique_ptr< + joint_limits::JointLimiterInterface> + limits_interface; + const joint_limits::JointLimits hard_limits{limits}; + joint_limits::JointInterfacesCommandLimiterData data; + data.joint_name = joint_name; + limiters_data_[hw_info.name].push_back(data); + // Check if soft limits exist. If yes then extract them and use soft limiter + if (hw_info.soft_limits.find(joint_name) != hw_info.soft_limits.end()) + { + std::vector soft_limits = { + hw_info.soft_limits.at(joint_name)}; + limits_interface = std::make_unique< + joint_limits::SoftJointLimiter>(); + } + // no soft limits given so we use the joint saturation limiter (joint_range_limiter.hpp) + else + { + limits_interface = std::make_unique< + joint_limits::JointSaturationLimiter>(); + + limits_interface->init({joint_name}, hard_limits, soft_limits, nullptr, nullptr); + } + joint_limiters_interface_[hw_info.name].push_back(std::move(limits_interface)); + } + } + } + + template + void update_joint_limiters_state( + const std::string & joint_name, const std::map & interface_map, + joint_limits::JointControlInterfacesData & state) + { + state.joint_name = joint_name; + // update the actual state of the limiters + if ( + interface_map.find(joint_name + "/" + hardware_interface::HW_IF_POSITION) != + interface_map.end()) + { + state.position = + interface_map.at(joint_name + "/" + hardware_interface::HW_IF_POSITION).get_value(); + } + if ( + interface_map.find(joint_name + "/" + hardware_interface::HW_IF_VELOCITY) != + interface_map.end()) + { + state.velocity = + interface_map.at(joint_name + "/" + hardware_interface::HW_IF_VELOCITY).get_value(); + } + if ( + interface_map.find(joint_name + "/" + hardware_interface::HW_IF_EFFORT) != + interface_map.end()) + { + state.effort = + interface_map.at(joint_name + "/" + hardware_interface::HW_IF_EFFORT).get_value(); + } + if ( + interface_map.find(joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) != + interface_map.end()) + { + state.acceleration = + interface_map.at(joint_name + "/" + hardware_interface::HW_IF_ACCELERATION).get_value(); + } + } + + template + void update_joint_limiters_commands( + const joint_limits::JointControlInterfacesData & state, + std::map & interface_map) + { + if ( + interface_map.find(state.joint_name + "/" + hardware_interface::HW_IF_POSITION) != + interface_map.end() && + state.position.has_value()) + { + interface_map.at(state.joint_name + "/" + hardware_interface::HW_IF_POSITION) + .set_value(state.position.value()); + } + if ( + interface_map.find(state.joint_name + "/" + hardware_interface::HW_IF_VELOCITY) != + interface_map.end() && + state.velocity.has_value()) + { + interface_map.at(state.joint_name + "/" + hardware_interface::HW_IF_VELOCITY) + .set_value(state.velocity.value()); + } + if ( + interface_map.find(state.joint_name + "/" + hardware_interface::HW_IF_EFFORT) != + interface_map.end() && + state.effort.has_value()) + { + interface_map.at(state.joint_name + "/" + hardware_interface::HW_IF_EFFORT) + .set_value(state.effort.value()); + } + if ( + interface_map.find(state.joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) != + interface_map.end() && + state.acceleration.has_value()) + { + interface_map.at(state.joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) + .set_value(state.acceleration.value()); + } + } + + void update_joint_limiters_data() + { + for (auto & joint_limiter_data : limiters_data_) + { + for (auto & data : joint_limiter_data.second) + { + update_joint_limiters_state(data.joint_name, state_interface_map_, data.actual); + update_joint_limiters_state(data.joint_name, command_interface_map_, data.command); + data.limited = data.command; + } + } + } + /// Adds exported command interfaces into internal storage. /** * Add command interfaces to the internal storage. Command interfaces exported from hardware or @@ -980,6 +1108,14 @@ class ResourceStorage /// List of async components by type std::unordered_map async_component_threads_; + std::unordered_map> + limiters_data_; + + std::unordered_map< + std::string, std::vector>>> + joint_limiters_interface_; + // Update rate of the controller manager, and the clock interface of its node // Used by async components. unsigned int cm_update_rate_; @@ -1022,6 +1158,7 @@ void ResourceManager::load_urdf( const std::string actuator_type = "actuator"; const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); + resource_storage_->import_joint_limiters(hardware_info); if (load_and_initialize_components) { for (const auto & individual_hardware_info : hardware_info) @@ -1689,6 +1826,22 @@ HardwareReadWriteStatus ResourceManager::write( read_write_status.ok = true; read_write_status.failed_hardware_names.clear(); + // Joint Limiters operations + { + resource_storage_->update_joint_limiters_data(); + for (auto & [hw_name, limiters] : resource_storage_->joint_limiters_interface_) + { + for (size_t i = 0; i < limiters.size(); i++) + { + joint_limits::JointInterfacesCommandLimiterData & data = + resource_storage_->limiters_data_[hw_name][i]; + limiters[i]->enforce(data.actual, data.limited, period); + resource_storage_->update_joint_limiters_commands( + data.limited, resource_storage_->command_interface_map_); + } + } + } + auto write_components = [&](auto & components) { for (auto & component : components) diff --git a/joint_limits/include/joint_limits/joint_soft_limiter.hpp b/joint_limits/include/joint_limits/joint_soft_limiter.hpp new file mode 100644 index 0000000000..61dc2b9ae0 --- /dev/null +++ b/joint_limits/include/joint_limits/joint_soft_limiter.hpp @@ -0,0 +1,70 @@ +// 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 Adrià Roig Moreno + +#ifndef JOINT_LIMITS__JOINT_SOFT_LIMITER_HPP_ +#define JOINT_LIMITS__JOINT_SOFT_LIMITER_HPP_ + +#include +#include "joint_limits/joint_limiter_struct.hpp" +#include "joint_limits/joint_limits_helpers.hpp" +#include "joint_limits/joint_saturation_limiter.hpp" + +constexpr double VALUE_CONSIDERED_ZERO = 1e-10; + +namespace joint_limits +{ + +class SoftJointLimiter : public JointSaturationLimiter +{ +public: + bool on_init() override + { + const bool result = (number_of_joints_ == 1); + if (!result && has_logging_interface()) + { + RCLCPP_ERROR( + node_logging_itf_->get_logger(), + "JointInterfacesSaturationLimiter: Expects the number of joints to be 1, but given : " + "%zu", + number_of_joints_); + } + prev_command_ = JointControlInterfacesData(); + return result; + } + + bool on_enforce( + JointControlInterfacesData & actual, JointControlInterfacesData & desired, + const rclcpp::Duration & dt) override; + + bool has_soft_position_limits(const joint_limits::SoftJointLimits & soft_joint_limits) + { + return std::isfinite(soft_joint_limits.min_position) && + std::isfinite(soft_joint_limits.max_position) && + (soft_joint_limits.max_position - soft_joint_limits.min_position) > + VALUE_CONSIDERED_ZERO; + } + + bool has_soft_limits(const joint_limits::SoftJointLimits & soft_joint_limits) + { + return has_soft_position_limits(soft_joint_limits) && + std::isfinite(soft_joint_limits.k_position) && + std::abs(soft_joint_limits.k_position) > VALUE_CONSIDERED_ZERO; + } +}; + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_SOFT_LIMITER_HPP_ diff --git a/joint_limits/src/soft_joint_limiter.cpp b/joint_limits/src/soft_joint_limiter.cpp index f209cb44b8..a71421f795 100644 --- a/joint_limits/src/soft_joint_limiter.cpp +++ b/joint_limits/src/soft_joint_limiter.cpp @@ -13,289 +13,248 @@ // limitations under the License. /// \author Adrià Roig Moreno - -#include -#include "joint_limits/joint_limiter_struct.hpp" -#include "joint_limits/joint_limits_helpers.hpp" -#include "joint_limits/joint_saturation_limiter.hpp" - -constexpr double VALUE_CONSIDERED_ZERO = 1e-10; +#include "joint_limits/joint_soft_limiter.hpp" namespace joint_limits { -class SoftJointLimiter : public JointSaturationLimiter +bool SoftJointLimiter::on_enforce( + JointControlInterfacesData & actual, JointControlInterfacesData & desired, + const rclcpp::Duration & dt) { -public: - bool on_init() + bool limits_enforced = false; + + const auto dt_seconds = dt.seconds(); + // negative or null is not allowed + if (dt_seconds <= 0.0) { - const bool result = (number_of_joints_ == 1); - if (!result && has_logging_interface()) - { - RCLCPP_ERROR( - node_logging_itf_->get_logger(), - "JointInterfacesSaturationLimiter: Expects the number of joints to be 1, but given : " - "%zu", - number_of_joints_); - } - prev_command_ = JointControlInterfacesData(); - return result; + return false; } - bool on_enforce( - JointControlInterfacesData & actual, JointControlInterfacesData & desired, - const rclcpp::Duration & dt) + const auto hard_limits = joint_limits_[0]; + joint_limits::SoftJointLimits soft_joint_limits; + if (!soft_joint_limits_.empty()) { - bool limits_enforced = false; + soft_joint_limits = soft_joint_limits_[0]; + } + + const std::string joint_name = joint_names_[0]; - const auto dt_seconds = dt.seconds(); - // negative or null is not allowed - if (dt_seconds <= 0.0) + if (!prev_command_.has_data()) + { + if (actual.has_position()) { - return false; + prev_command_.position = actual.position; } - - const auto hard_limits = joint_limits_[0]; - joint_limits::SoftJointLimits soft_joint_limits; - if (!soft_joint_limits_.empty()) + else if (desired.has_position()) { - soft_joint_limits = soft_joint_limits_[0]; + prev_command_.position = desired.position; } - - const std::string joint_name = joint_names_[0]; - - if (!prev_command_.has_data()) + if (actual.has_velocity()) { - if (actual.has_position()) - { - prev_command_.position = actual.position; - } - else if (desired.has_position()) - { - prev_command_.position = desired.position; - } - if (actual.has_velocity()) - { - prev_command_.velocity = actual.velocity; - } - else if (desired.has_velocity()) - { - prev_command_.velocity = desired.velocity; - } - if (actual.has_effort()) - { - prev_command_.effort = actual.effort; - } - else if (desired.has_effort()) - { - prev_command_.effort = desired.effort; - } - if (actual.has_acceleration()) - { - prev_command_.acceleration = actual.acceleration; - } - else if (desired.has_acceleration()) - { - prev_command_.acceleration = desired.acceleration; - } - if (actual.has_jerk()) - { - prev_command_.jerk = actual.jerk; - } - else if (desired.has_jerk()) - { - prev_command_.jerk = desired.jerk; - } - if (actual.has_data()) - { - prev_command_.joint_name = actual.joint_name; - } - else if (desired.has_data()) - { - prev_command_.joint_name = desired.joint_name; - } + prev_command_.velocity = actual.velocity; } - - double soft_min_vel = -std::numeric_limits::infinity(); - double soft_max_vel = std::numeric_limits::infinity(); - double position = std::numeric_limits::infinity(); - - if (prev_command_.has_position() && std::isfinite(prev_command_.position.value())) + else if (desired.has_velocity()) { - position = prev_command_.position.value(); + prev_command_.velocity = desired.velocity; } - else if (actual.has_position()) + if (actual.has_effort()) { - position = actual.position.value(); + prev_command_.effort = actual.effort; } - - if (hard_limits.has_velocity_limits) + else if (desired.has_effort()) { - soft_min_vel = -hard_limits.max_velocity; - soft_max_vel = hard_limits.max_velocity; - - if ( - hard_limits.has_position_limits && has_soft_limits(soft_joint_limits) && - std::isfinite(position)) - { - soft_min_vel = std::clamp( - -soft_joint_limits.k_position * (position - soft_joint_limits.min_position), - -hard_limits.max_velocity, hard_limits.max_velocity); - - soft_max_vel = std::clamp( - -soft_joint_limits.k_position * (position - soft_joint_limits.max_position), - -hard_limits.max_velocity, hard_limits.max_velocity); - - if ((position < hard_limits.min_position) || (position > hard_limits.max_position)) - { - soft_min_vel = 0.0; - soft_max_vel = 0.0; - } - else if ( - (position < soft_joint_limits.min_position) || - (position > soft_joint_limits.max_position)) - { - constexpr double soft_limit_reach_velocity = 1.0 * (M_PI / 180.0); - soft_min_vel = std::copysign(soft_limit_reach_velocity, soft_min_vel); - soft_max_vel = std::copysign(soft_limit_reach_velocity, soft_max_vel); - } - } + prev_command_.effort = desired.effort; + } + if (actual.has_acceleration()) + { + prev_command_.acceleration = actual.acceleration; } + else if (desired.has_acceleration()) + { + prev_command_.acceleration = desired.acceleration; + } + if (actual.has_jerk()) + { + prev_command_.jerk = actual.jerk; + } + else if (desired.has_jerk()) + { + prev_command_.jerk = desired.jerk; + } + if (actual.has_data()) + { + prev_command_.joint_name = actual.joint_name; + } + else if (desired.has_data()) + { + prev_command_.joint_name = desired.joint_name; + } + } - if (desired.has_position()) + double soft_min_vel = -std::numeric_limits::infinity(); + double soft_max_vel = std::numeric_limits::infinity(); + double position = std::numeric_limits::infinity(); + + if (prev_command_.has_position() && std::isfinite(prev_command_.position.value())) + { + position = prev_command_.position.value(); + } + else if (actual.has_position()) + { + position = actual.position.value(); + } + + if (hard_limits.has_velocity_limits) + { + soft_min_vel = -hard_limits.max_velocity; + soft_max_vel = hard_limits.max_velocity; + + if ( + hard_limits.has_position_limits && has_soft_limits(soft_joint_limits) && + std::isfinite(position)) { - const auto position_limits = - compute_position_limits(hard_limits, actual.velocity, prev_command_.position, dt_seconds); + soft_min_vel = std::clamp( + -soft_joint_limits.k_position * (position - soft_joint_limits.min_position), + -hard_limits.max_velocity, hard_limits.max_velocity); - double pos_low = -std::numeric_limits::infinity(); - double pos_high = std::numeric_limits::infinity(); + soft_max_vel = std::clamp( + -soft_joint_limits.k_position * (position - soft_joint_limits.max_position), + -hard_limits.max_velocity, hard_limits.max_velocity); - if (has_soft_position_limits(soft_joint_limits)) + if ((position < hard_limits.min_position) || (position > hard_limits.max_position)) { - pos_low = soft_joint_limits.min_position; - pos_high = soft_joint_limits.max_position; + soft_min_vel = 0.0; + soft_max_vel = 0.0; } - - if (hard_limits.has_velocity_limits) + else if ( + (position < soft_joint_limits.min_position) || (position > soft_joint_limits.max_position)) { - pos_low = std::clamp(position + soft_min_vel * dt_seconds, pos_low, pos_high); - pos_high = std::clamp(position + soft_max_vel * dt_seconds, pos_low, pos_high); + constexpr double soft_limit_reach_velocity = 1.0 * (M_PI / 180.0); + soft_min_vel = std::copysign(soft_limit_reach_velocity, soft_min_vel); + soft_max_vel = std::copysign(soft_limit_reach_velocity, soft_max_vel); } - pos_low = std::max(pos_low, position_limits.first); - pos_high = std::min(pos_high, position_limits.second); - - limits_enforced = is_limited(desired.position.value(), pos_low, pos_high); - desired.position = std::clamp(desired.position.value(), pos_low, pos_high); } + } - if (desired.has_velocity()) - { - const auto velocity_limits = compute_velocity_limits( - joint_name, hard_limits, actual.position, prev_command_.velocity, dt_seconds); - - if (hard_limits.has_acceleration_limits && actual.has_velocity()) - { - soft_min_vel = std::max( - actual.velocity.value() - hard_limits.max_acceleration * dt_seconds, soft_min_vel); - soft_max_vel = std::min( - actual.velocity.value() + hard_limits.max_acceleration * dt_seconds, soft_max_vel); - } + if (desired.has_position()) + { + const auto position_limits = + compute_position_limits(hard_limits, actual.velocity, prev_command_.position, dt_seconds); - soft_min_vel = std::max(soft_min_vel, velocity_limits.first); - soft_max_vel = std::min(soft_max_vel, velocity_limits.second); + double pos_low = -std::numeric_limits::infinity(); + double pos_high = std::numeric_limits::infinity(); - limits_enforced = - is_limited(desired.velocity.value(), soft_min_vel, soft_max_vel) || limits_enforced; - desired.velocity = std::clamp(desired.velocity.value(), soft_min_vel, soft_max_vel); + if (has_soft_position_limits(soft_joint_limits)) + { + pos_low = soft_joint_limits.min_position; + pos_high = soft_joint_limits.max_position; } - if (desired.has_effort()) + if (hard_limits.has_velocity_limits) { - const auto effort_limits = - compute_effort_limits(hard_limits, actual.position, actual.velocity, dt_seconds); + pos_low = std::clamp(position + soft_min_vel * dt_seconds, pos_low, pos_high); + pos_high = std::clamp(position + soft_max_vel * dt_seconds, pos_low, pos_high); + } + pos_low = std::max(pos_low, position_limits.first); + pos_high = std::min(pos_high, position_limits.second); - double soft_min_eff = effort_limits.first; - double soft_max_eff = effort_limits.second; + limits_enforced = is_limited(desired.position.value(), pos_low, pos_high); + desired.position = std::clamp(desired.position.value(), pos_low, pos_high); + } - if ( - hard_limits.has_effort_limits && std::isfinite(soft_joint_limits.k_velocity) && - actual.has_velocity()) - { - soft_min_eff = std::clamp( - -soft_joint_limits.k_velocity * (actual.velocity.value() - soft_min_vel), - -hard_limits.max_effort, hard_limits.max_effort); + if (desired.has_velocity()) + { + const auto velocity_limits = compute_velocity_limits( + joint_name, hard_limits, actual.position, prev_command_.velocity, dt_seconds); + + if (hard_limits.has_acceleration_limits && actual.has_velocity()) + { + soft_min_vel = + std::max(actual.velocity.value() - hard_limits.max_acceleration * dt_seconds, soft_min_vel); + soft_max_vel = + std::min(actual.velocity.value() + hard_limits.max_acceleration * dt_seconds, soft_max_vel); + } - soft_max_eff = std::clamp( - -soft_joint_limits.k_velocity * (actual.velocity.value() - soft_max_vel), - -hard_limits.max_effort, hard_limits.max_effort); + soft_min_vel = std::max(soft_min_vel, velocity_limits.first); + soft_max_vel = std::min(soft_max_vel, velocity_limits.second); - soft_min_eff = std::max(soft_min_eff, effort_limits.first); - soft_max_eff = std::min(soft_max_eff, effort_limits.second); - } + limits_enforced = + is_limited(desired.velocity.value(), soft_min_vel, soft_max_vel) || limits_enforced; + desired.velocity = std::clamp(desired.velocity.value(), soft_min_vel, soft_max_vel); + } - limits_enforced = - is_limited(desired.effort.value(), soft_min_eff, soft_max_eff) || limits_enforced; - desired.effort = std::clamp(desired.effort.value(), soft_min_eff, soft_max_eff); - } + if (desired.has_effort()) + { + const auto effort_limits = + compute_effort_limits(hard_limits, actual.position, actual.velocity, dt_seconds); - if (desired.has_acceleration()) - { - const auto limits = - compute_acceleration_limits(hard_limits, desired.acceleration.value(), actual.velocity); - limits_enforced = - is_limited(desired.acceleration.value(), limits.first, limits.second) || limits_enforced; - desired.acceleration = std::clamp(desired.acceleration.value(), limits.first, limits.second); - } + double soft_min_eff = effort_limits.first; + double soft_max_eff = effort_limits.second; - if (desired.has_jerk()) + if ( + hard_limits.has_effort_limits && std::isfinite(soft_joint_limits.k_velocity) && + actual.has_velocity()) { - limits_enforced = - is_limited(desired.jerk.value(), -hard_limits.max_jerk, hard_limits.max_jerk) || - limits_enforced; - desired.jerk = std::clamp(desired.jerk.value(), -hard_limits.max_jerk, hard_limits.max_jerk); - } + soft_min_eff = std::clamp( + -soft_joint_limits.k_velocity * (actual.velocity.value() - soft_min_vel), + -hard_limits.max_effort, hard_limits.max_effort); - if (desired.has_position() && !std::isfinite(desired.position.value()) && actual.has_position()) - { - desired.position = actual.position; - limits_enforced = true; - } - if (desired.has_velocity() && !std::isfinite(desired.velocity.value())) - { - desired.velocity = 0.0; - limits_enforced = true; - } - if (desired.has_acceleration() && !std::isfinite(desired.acceleration.value())) - { - desired.acceleration = 0.0; - limits_enforced = true; - } - if (desired.has_jerk() && !std::isfinite(desired.jerk.value())) - { - desired.jerk = 0.0; - limits_enforced = true; + soft_max_eff = std::clamp( + -soft_joint_limits.k_velocity * (actual.velocity.value() - soft_max_vel), + -hard_limits.max_effort, hard_limits.max_effort); + + soft_min_eff = std::max(soft_min_eff, effort_limits.first); + soft_max_eff = std::min(soft_max_eff, effort_limits.second); } - prev_command_ = desired; + limits_enforced = + is_limited(desired.effort.value(), soft_min_eff, soft_max_eff) || limits_enforced; + desired.effort = std::clamp(desired.effort.value(), soft_min_eff, soft_max_eff); + } - return limits_enforced; + if (desired.has_acceleration()) + { + const auto limits = + compute_acceleration_limits(hard_limits, desired.acceleration.value(), actual.velocity); + limits_enforced = + is_limited(desired.acceleration.value(), limits.first, limits.second) || limits_enforced; + desired.acceleration = std::clamp(desired.acceleration.value(), limits.first, limits.second); } - bool has_soft_position_limits(const joint_limits::SoftJointLimits & soft_joint_limits) + if (desired.has_jerk()) { - return std::isfinite(soft_joint_limits.min_position) && - std::isfinite(soft_joint_limits.max_position) && - (soft_joint_limits.max_position - soft_joint_limits.min_position) > - VALUE_CONSIDERED_ZERO; + limits_enforced = + is_limited(desired.jerk.value(), -hard_limits.max_jerk, hard_limits.max_jerk) || + limits_enforced; + desired.jerk = std::clamp(desired.jerk.value(), -hard_limits.max_jerk, hard_limits.max_jerk); } - bool has_soft_limits(const joint_limits::SoftJointLimits & soft_joint_limits) + if (desired.has_position() && !std::isfinite(desired.position.value()) && actual.has_position()) + { + desired.position = actual.position; + limits_enforced = true; + } + if (desired.has_velocity() && !std::isfinite(desired.velocity.value())) { - return has_soft_position_limits(soft_joint_limits) && - std::isfinite(soft_joint_limits.k_position) && - std::abs(soft_joint_limits.k_position) > VALUE_CONSIDERED_ZERO; + desired.velocity = 0.0; + limits_enforced = true; } -}; + if (desired.has_acceleration() && !std::isfinite(desired.acceleration.value())) + { + desired.acceleration = 0.0; + limits_enforced = true; + } + if (desired.has_jerk() && !std::isfinite(desired.jerk.value())) + { + desired.jerk = 0.0; + limits_enforced = true; + } + + prev_command_ = desired; + + return limits_enforced; +} } // namespace joint_limits