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