diff --git a/joint_limits_interface/CMakeLists.txt b/joint_limits_interface/CMakeLists.txt deleted file mode 100644 index a5d4577343..0000000000 --- a/joint_limits_interface/CMakeLists.txt +++ /dev/null @@ -1,47 +0,0 @@ -cmake_minimum_required(VERSION 3.5.0) -project(joint_limits_interface) - -find_package(ament_cmake REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(rclcpp REQUIRED) -find_package(urdf REQUIRED) - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - find_package(launch_testing_ament_cmake REQUIRED) - - ament_add_gtest(joint_limits_interface_test test/joint_limits_interface_test.cpp) - target_include_directories(joint_limits_interface_test PUBLIC include) - ament_target_dependencies(joint_limits_interface_test hardware_interface rclcpp) - - add_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) - target_include_directories(joint_limits_rosparam_test PUBLIC include ${GTEST_INCLUDE_DIRS}) - target_link_libraries(joint_limits_rosparam_test ${GTEST_LIBRARIES}) - ament_target_dependencies(joint_limits_rosparam_test rclcpp) - add_launch_test(test/joint_limits_rosparam.launch.py) - install( - TARGETS - joint_limits_rosparam_test - DESTINATION lib/${PROJECT_NAME} - ) - install( - FILES - test/joint_limits_rosparam.yaml - DESTINATION share/${PROJECT_NAME}/test - ) - - ament_add_gtest(joint_limits_urdf_test test/joint_limits_urdf_test.cpp) - target_include_directories(joint_limits_urdf_test PUBLIC include) - ament_target_dependencies(joint_limits_urdf_test rclcpp urdf) -endif() - -# Install headers -install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include/${PROJECT_NAME} -) - -ament_export_include_directories(include) -ament_export_dependencies(rclcpp urdf) - -ament_package() diff --git a/joint_limits_interface/COLCON_IGNORE b/joint_limits_interface/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/joint_limits_interface/README.md b/joint_limits_interface/README.md deleted file mode 100644 index c77ccc2b75..0000000000 --- a/joint_limits_interface/README.md +++ /dev/null @@ -1,30 +0,0 @@ -## Joint Limits Interface ## - -### Overview ### - -**joint_limits_interface** contains data structures for representing joint limits, methods for populating them from common -formats such as URDF and the ROS parameter server, and methods for enforcing limits on different kinds of hardware interfaces. - -The **joint_limits_interface** is *not* used by controllers themselves (it does not implement a `HardwareInterface`) but -instead operates after the controllers have updated, in the `write()` method (or equivalent) of the robot abstraction. -Enforcing limits will *overwrite* the commands set by the controllers, it does not operate on a separate raw data buffer. - -There are two main elements involved in setting up a joint limits interface: - - - **Joint limits representation** - - **JointLimits** Position, velocity, acceleration, jerk and effort. - - **SoftJointLimits** Soft position limits, `k_p`, `k_v` (as described [here](http://www.ros.org/wiki/pr2_controller_manager/safety_limits)). - - **Loading from URDF** There are convenience methods for loading joint limits information - (position, velocity and effort), as well as soft joint limits information from the URDF. - - **Loading from ROS params** There are convenience methods for loading joint limits from the ROS parameter server - (position, velocity, acceleration, jerk and effort). Parameter specification is the same used in MoveIt, - with the addition that we also parse jerk and effort limits. - - - **Joint limits interface** - - - For **effort-controlled** joints, the soft-limits implementation from the PR2 has been ported. - - For **position-controlled** joints, a modified version of the PR2 soft limits has been implemented. - - For **velocity-controlled** joints, simple saturation based on acceleration and velocity limits has been implemented. - -### Examples ### -Please refer to the [joint_limits_interface](https://github.com/ros-controls/ros_control/wiki/joint_limits_interface) wiki page. diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp deleted file mode 100644 index db553948d1..0000000000 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp +++ /dev/null @@ -1,718 +0,0 @@ -// Copyright 2020 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 Adolfo Rodriguez Tsouroukdissian - -#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ -#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ - -#include -#include -#include -#include -#include -#include - -#include "joint_limits_interface/joint_limits.hpp" -#include "joint_limits_interface/joint_limits_interface_exception.hpp" - -#include -#include - -#include -#include - -namespace joint_limits_interface -{ -/** - * The base class of limit handles for enforcing position, velocity, and effort limits of - * an effort-controlled joint. - */ -class JointLimitHandle -{ -public: - JointLimitHandle() - : prev_pos_(std::numeric_limits::quiet_NaN()), - prev_vel_(0.0), - jposh_(hardware_interface::HW_IF_POSITION), - jvelh_(hardware_interface::HW_IF_VELOCITY), - jcmdh_("position_command") - { - } - - JointLimitHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh, - const JointLimits & limits) - : jposh_(jposh), - jvelh_(hardware_interface::HW_IF_VELOCITY), - jcmdh_(jcmdh), - limits_(limits), - prev_pos_(std::numeric_limits::quiet_NaN()), - prev_vel_(0.0) - { - } - - JointLimitHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, const JointLimits & limits) - : jposh_(jposh), - jvelh_(jvelh), - jcmdh_(jcmdh), - limits_(limits), - prev_pos_(std::numeric_limits::quiet_NaN()), - prev_vel_(0.0) - { - } - - /// \return Joint name. - std::string get_name() const - { - return jposh_ ? jposh_.get_name() - : jvelh_ ? jvelh_.get_name() - : jcmdh_ ? jcmdh_.get_name() - : std::string(); - } - - /// Sub-class implementation of limit enforcing policy. - virtual void enforce_limits(const rclcpp::Duration & period) = 0; - - /// Clear stored state, causing it to reset next iteration. - virtual void reset() - { - prev_pos_ = std::numeric_limits::quiet_NaN(); - prev_vel_ = 0.0; - } - -protected: - hardware_interface::JointHandle jposh_; - hardware_interface::JointHandle jvelh_; - hardware_interface::JointHandle jcmdh_; - joint_limits_interface::JointLimits limits_; - - // stored state - track position and velocity of last update - double prev_pos_; - double prev_vel_; - - /// Return velocity for limit calculations. - /** - * @param period Time since last measurement - * @return the velocity, from state if available, otherwise from previous position history. - */ - double get_velocity(const rclcpp::Duration & period) const - { - // if we have a handle to a velocity state we can directly return state velocity - // otherwise we will estimate velocity from previous position (command or state) - return jvelh_ ? jvelh_.get_value() : (jposh_.get_value() - prev_pos_) / period.seconds(); - } -}; - -/** The base class of limit handles for enforcing position, velocity, and effort limits of - * an effort-controlled joint that has soft-limits. - */ -class JointSoftLimitsHandle : public JointLimitHandle -{ -public: - JointSoftLimitsHandle() {} - - JointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh, - const JointLimits & limits, const SoftJointLimits & soft_limits) - : JointLimitHandle(jposh, jcmdh, limits), soft_limits_(soft_limits) - { - } - - JointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, const JointLimits & limits, - const SoftJointLimits & soft_limits) - : JointLimitHandle(jposh, jvelh, jcmdh, limits), soft_limits_(soft_limits) - { - } - -protected: - joint_limits_interface::SoftJointLimits soft_limits_; -}; - -/** A handle used to enforce position and velocity limits of a position-controlled joint that does - not have soft limits. */ -class PositionJointSaturationHandle : public JointLimitHandle -{ -public: - PositionJointSaturationHandle() {} - - PositionJointSaturationHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh, - const JointLimits & limits) - : JointLimitHandle(jposh, jcmdh, limits) - { - if (limits_.has_position_limits) - { - min_pos_limit_ = limits_.min_position; - max_pos_limit_ = limits_.max_position; - } - else - { - min_pos_limit_ = -std::numeric_limits::max(); - max_pos_limit_ = std::numeric_limits::max(); - } - } - - /// Enforce position and velocity limits for a joint that is not subject to soft limits. - /** - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) - { - if (std::isnan(prev_pos_)) - { - prev_pos_ = jposh_.get_value(); - } - - double min_pos, max_pos; - if (limits_.has_velocity_limits) - { - // enforce velocity limits - // set constraints on where the position can be based on the - // max velocity times seconds since last update - const double delta_pos = limits_.max_velocity * period.seconds(); - min_pos = std::max(prev_pos_ - delta_pos, min_pos_limit_); - max_pos = std::min(prev_pos_ + delta_pos, max_pos_limit_); - } - else - { - // no velocity limit, so position is simply limited to set extents (our imposed soft limits) - min_pos = min_pos_limit_; - max_pos = max_pos_limit_; - } - - // clamp command position to our computed min/max position - const double cmd = std::clamp(jcmdh_.get_value(), min_pos, max_pos); - jcmdh_.set_value(cmd); - - prev_pos_ = cmd; - } - -private: - double min_pos_limit_, max_pos_limit_; -}; - -/// A handle used to enforce position and velocity limits of a position-controlled joint. -/** - * This class implements a very simple position and velocity limits enforcing policy, and tries to - * impose the least amount of requisites on the underlying hardware platform. This lowers - * considerably the entry barrier to use it, but also implies some limitations. - * - * Requisites - * - Position (for non-continuous joints) and velocity limits specification. - * - Soft limits specification. The \c k_velocity parameter is \e not used. - * - * Open loop nature - * - * Joint position and velocity limits are enforced in an open-loop fashion, that is, the command is - * checked for validity without relying on the actual position/velocity values. - * - * - Actual position values are \e not used because in some platforms there might be a substantial - * lag between sending a command and executing it (propagate command to hardware, reach control - * objective, read from hardware). - * - * - Actual velocity values are \e not used because of the above reason, and because some platforms - * might not expose trustworthy velocity measurements, or none at all. - * - * The downside of the open loop behavior is that velocity limits will not be enforced when - * recovering from large position tracking errors. Only the command is guaranteed to comply with the - * limits specification. - * - * \note: This handle type is \e stateful, ie. it stores the previous position command to estimate - * the command velocity. - */ - -// TODO(anyone): Leverage %Reflexxes Type II library for acceleration limits handling? -class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle -{ -public: - PositionJointSoftLimitsHandle() {} - - PositionJointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(jposh, jcmdh, limits, soft_limits) - { - if (!limits.has_velocity_limits) - { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - } - - /// Enforce position and velocity limits for a joint subject to soft limits. - /** - * If the joint has no position limits (eg. a continuous joint), only velocity limits will be - * enforced. - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - assert(period.seconds() > 0.0); - - // Current position - if (std::isnan(prev_pos_)) - { - // Happens only once at initialization - prev_pos_ = jposh_.get_value(); - } - const double pos = prev_pos_; - - // Velocity bounds - double soft_min_vel; - double soft_max_vel; - - if (limits_.has_position_limits) - { - // Velocity bounds depend on the velocity limit and the proximity to the position limit - soft_min_vel = std::clamp( - -soft_limits_.k_position * (pos - soft_limits_.min_position), -limits_.max_velocity, - limits_.max_velocity); - - soft_max_vel = std::clamp( - -soft_limits_.k_position * (pos - soft_limits_.max_position), -limits_.max_velocity, - limits_.max_velocity); - } - else - { - // No position limits, eg. continuous joints - soft_min_vel = -limits_.max_velocity; - soft_max_vel = limits_.max_velocity; - } - - // Position bounds - const double dt = period.seconds(); - double pos_low = pos + soft_min_vel * dt; - double pos_high = pos + soft_max_vel * dt; - - if (limits_.has_position_limits) - { - // This extra measure safeguards against pathological cases, like when the soft limit lies - // beyond the hard limit - pos_low = std::max(pos_low, limits_.min_position); - pos_high = std::min(pos_high, limits_.max_position); - } - - // Saturate position command according to bounds - const double pos_cmd = std::clamp(jcmdh_.get_value(), pos_low, pos_high); - jcmdh_.set_value(pos_cmd); - - // Cache variables - // todo: shouldn't this just be pos_cmd? why call into the command handle to - // get what we have in the above line? - prev_pos_ = jcmdh_.get_value(); - } -}; - -/** - * A handle used to enforce position, velocity, and effort limits of an effort-controlled - * joint that does not have soft limits. - */ -class EffortJointSaturationHandle : public JointLimitHandle -{ -public: - EffortJointSaturationHandle() {} - - EffortJointSaturationHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits) - : JointLimitHandle(jposh, jvelh, jcmdh, limits) - { - if (!limits.has_velocity_limits) - { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - if (!limits.has_effort_limits) - { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no efforts limits specification."); - } - } - - EffortJointSaturationHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits) - : EffortJointSaturationHandle( - jposh, hardware_interface::JointHandle(hardware_interface::HW_IF_VELOCITY), jcmdh, limits) - { - } - - /** - * Enforce position, velocity, and effort limits for a joint that is not subject - * to soft limits. - * - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - double min_eff = -limits_.max_effort; - double max_eff = limits_.max_effort; - - if (limits_.has_position_limits) - { - const double pos = jposh_.get_value(); - if (pos < limits_.min_position) - { - min_eff = 0.0; - } - else if (pos > limits_.max_position) - { - max_eff = 0.0; - } - } - - const double vel = get_velocity(period); - if (vel < -limits_.max_velocity) - { - min_eff = 0.0; - } - else if (vel > limits_.max_velocity) - { - max_eff = 0.0; - } - - double clamped = std::clamp(jcmdh_.get_value(), min_eff, max_eff); - jcmdh_.set_value(clamped); - } -}; - -/// A handle used to enforce position, velocity and effort limits of an effort-controlled joint. -// TODO(anyone): This class is untested!. Update unit tests accordingly. -class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle -{ -public: - EffortJointSoftLimitsHandle() {} - - EffortJointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(jposh, jvelh, jcmdh, limits, soft_limits) - { - if (!limits.has_velocity_limits) - { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - if (!limits.has_effort_limits) - { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no effort limits specification."); - } - } - - EffortJointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : EffortJointSoftLimitsHandle( - jposh, hardware_interface::JointHandle(hardware_interface::HW_IF_VELOCITY), jcmdh, limits, - soft_limits) - { - } - - /// Enforce position, velocity and effort limits for a joint subject to soft limits. - /** - * If the joint has no position limits (eg. a continuous joint), only velocity and effort limits - * will be enforced. - * - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - // Current state - const double pos = jposh_.get_value(); - const double vel = get_velocity(period); - - // Velocity bounds - double soft_min_vel; - double soft_max_vel; - - if (limits_.has_position_limits) - { - // Velocity bounds depend on the velocity limit and the proximity to the position limit - soft_min_vel = std::clamp( - -soft_limits_.k_position * (pos - soft_limits_.min_position), -limits_.max_velocity, - limits_.max_velocity); - - soft_max_vel = std::clamp( - -soft_limits_.k_position * (pos - soft_limits_.max_position), -limits_.max_velocity, - limits_.max_velocity); - } - else - { - // No position limits, eg. continuous joints - soft_min_vel = -limits_.max_velocity; - soft_max_vel = limits_.max_velocity; - } - - // Effort bounds depend on the velocity and effort bounds - const double soft_min_eff = std::clamp( - -soft_limits_.k_velocity * (vel - soft_min_vel), -limits_.max_effort, limits_.max_effort); - - const double soft_max_eff = std::clamp( - -soft_limits_.k_velocity * (vel - soft_max_vel), -limits_.max_effort, limits_.max_effort); - - // Saturate effort command according to bounds - const double eff_cmd = std::clamp(jcmdh_.get_value(), soft_min_eff, soft_max_eff); - jcmdh_.set_value(eff_cmd); - } -}; - -/// A handle used to enforce velocity and acceleration limits of a velocity-controlled joint. -class VelocityJointSaturationHandle : public JointLimitHandle -{ -public: - VelocityJointSaturationHandle() {} - - VelocityJointSaturationHandle( - const hardware_interface::JointHandle & jvelh, // currently unused - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits) - : JointLimitHandle( - hardware_interface::JointHandle(hardware_interface::HW_IF_POSITION), jvelh, jcmdh, limits) - { - if (!limits.has_velocity_limits) - { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - } - - VelocityJointSaturationHandle( - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits) - : JointLimitHandle( - hardware_interface::JointHandle(hardware_interface::HW_IF_POSITION), - hardware_interface::JointHandle(hardware_interface::HW_IF_VELOCITY), jcmdh, limits) - { - if (!limits.has_velocity_limits) - { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - } - - /// Enforce joint velocity and acceleration limits. - /** - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - // Velocity bounds - double vel_low; - double vel_high; - - if (limits_.has_acceleration_limits) - { - assert(period.seconds() > 0.0); - const double dt = period.seconds(); - - vel_low = std::max(prev_vel_ - limits_.max_acceleration * dt, -limits_.max_velocity); - vel_high = std::min(prev_vel_ + limits_.max_acceleration * dt, limits_.max_velocity); - } - else - { - vel_low = -limits_.max_velocity; - vel_high = limits_.max_velocity; - } - - // Saturate velocity command according to limits - const double vel_cmd = std::clamp(jcmdh_.get_value(), vel_low, vel_high); - jcmdh_.set_value(vel_cmd); - - // Cache variables - prev_vel_ = jcmdh_.get_value(); - } -}; - -/** - * A handle used to enforce position, velocity, and acceleration limits of a - * velocity-controlled joint. - */ -class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle -{ -public: - VelocityJointSoftLimitsHandle() {} - - VelocityJointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(jposh, jvelh, jcmdh, limits, soft_limits) - { - if (limits.has_velocity_limits) - { - max_vel_limit_ = limits.max_velocity; - } - else - { - max_vel_limit_ = std::numeric_limits::max(); - } - } - - /** - * Enforce position, velocity, and acceleration limits for a velocity-controlled joint - * subject to soft limits. - * - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) - { - double min_vel, max_vel; - if (limits_.has_position_limits) - { - // Velocity bounds depend on the velocity limit and the proximity to the position limit. - const double pos = jposh_.get_value(); - min_vel = std::clamp( - -soft_limits_.k_position * (pos - soft_limits_.min_position), -max_vel_limit_, - max_vel_limit_); - max_vel = std::clamp( - -soft_limits_.k_position * (pos - soft_limits_.max_position), -max_vel_limit_, - max_vel_limit_); - } - else - { - min_vel = -max_vel_limit_; - max_vel = max_vel_limit_; - } - - if (limits_.has_acceleration_limits) - { - const double vel = get_velocity(period); - const double delta_t = period.seconds(); - min_vel = std::max(vel - limits_.max_acceleration * delta_t, min_vel); - max_vel = std::min(vel + limits_.max_acceleration * delta_t, max_vel); - } - - jcmdh_.set_value(std::clamp(jcmdh_.get_value(), min_vel, max_vel)); - } - -private: - double max_vel_limit_; -}; - -// TODO(anyone): Port this to ROS 2 -// //** -// * Interface for enforcing joint limits. -// * -// * \tparam HandleType %Handle type. Must implement the following methods: -// * \code -// * void enforce_limits(); -// * std::string get_name() const; -// * \endcode -// */ -// template -// class joint_limits_interface::JointLimitsInterface -// : public hardware_interface::ResourceManager -// { -// public: -// HandleType getHandle(const std::string & name) -// { -// // Rethrow exception with a meaningful type -// try { -// return this->hardware_interface::ResourceManager::getHandle(name); -// } catch (const std::logic_error & e) { -// throw joint_limits_interface::JointLimitsInterfaceException(e.what()); -// } -// } -// -// /** \name Real-Time Safe Functions -// *\{*/ -// /** Enforce limits for all managed handles. */ -// void enforce_limits(const rclcpp::Duration & period) -// { -// for (auto && resource_name_and_handle : this->resource_map_) { -// resource_name_and_handle.second.enforce_limits(period); -// } -// } -// /*\}*/ -// }; -// -// /** Interface for enforcing limits on a position-controlled joint through saturation. */ -// class PositionJointSaturationInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// public: -// /** \name Real-Time Safe Functions -// *\{*/ -// /** Reset all managed handles. */ -// void reset() -// { -// for (auto && resource_name_and_handle : this->resource_map_) { -// resource_name_and_handle.second.reset(); -// } -// } -// /*\}*/ -// }; -// -// /** Interface for enforcing limits on a position-controlled joint with soft position limits. */ -// class PositionJointSoftLimitsInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// public: -// /** \name Real-Time Safe Functions -// *\{*/ -// /** Reset all managed handles. */ -// void reset() -// { -// for (auto && resource_name_and_handle : this->resource_map_) { -// resource_name_and_handle.second.reset(); -// } -// } -// /*\}*/ -// }; -// -// /** Interface for enforcing limits on an effort-controlled joint through saturation. */ -// class EffortJointSaturationInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -// -// /** Interface for enforcing limits on an effort-controlled joint with soft position limits. */ -// class EffortJointSoftLimitsInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -// -// /** Interface for enforcing limits on a velocity-controlled joint through saturation. */ -// class VelocityJointSaturationInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -// -// /** Interface for enforcing limits on a velocity-controlled joint with soft position limits. */ -// class VelocityJointSoftLimitsInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -} // namespace joint_limits_interface - -#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface_exception.hpp b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface_exception.hpp deleted file mode 100644 index 430f46b48e..0000000000 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface_exception.hpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2020 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. - -#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_EXCEPTION_HPP_ -#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_EXCEPTION_HPP_ - -#include - -namespace joint_limits_interface -{ -/// An exception related to a \ref JointLimitsInterface -class JointLimitsInterfaceException : public std::exception -{ -public: - explicit JointLimitsInterfaceException(const std::string & message) : msg(message) {} - - const char * what() const noexcept override { return msg.c_str(); } - -private: - std::string msg; -}; - -} // namespace joint_limits_interface - -#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_EXCEPTION_HPP_ diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp b/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp deleted file mode 100644 index 80cf7f0ed4..0000000000 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp +++ /dev/null @@ -1,88 +0,0 @@ -// Copyright 2020 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 Adolfo Rodriguez Tsouroukdissian - -#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_URDF_HPP_ -#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_URDF_HPP_ - -#include -#include -#include -#include - -namespace joint_limits_interface -{ -/** - * Populate a JointLimits instance from URDF joint data. - * \param[in] urdf_joint URDF joint. - * \param[out] limits Where URDF joint limit data gets written into. Limits in \e urdf_joint will - * overwrite existing values. Values in \e limits not present in \e urdf_joint remain unchanged. - * \return True if \e urdf_joint has a valid limits specification, false otherwise. - */ -inline bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits & limits) -{ - if (!urdf_joint || !urdf_joint->limits) - { - return false; - } - - limits.has_position_limits = - urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::PRISMATIC; - if (limits.has_position_limits) - { - limits.min_position = urdf_joint->limits->lower; - limits.max_position = urdf_joint->limits->upper; - } - - if (!limits.has_position_limits && urdf_joint->type == urdf::Joint::CONTINUOUS) - { - limits.angle_wraparound = true; - } - - limits.has_velocity_limits = true; - limits.max_velocity = urdf_joint->limits->velocity; - - limits.has_acceleration_limits = false; - - limits.has_effort_limits = true; - limits.max_effort = urdf_joint->limits->effort; - - return true; -} - -/** - * Populate a SoftJointLimits instance from URDF joint data. - * \param[in] urdf_joint URDF joint. - * \param[out] soft_limits Where URDF soft joint limit data gets written into. - * \return True if \e urdf_joint has a valid soft limits specification, false otherwise. - */ -inline bool getSoftJointLimits(urdf::JointConstSharedPtr urdf_joint, SoftJointLimits & soft_limits) -{ - if (!urdf_joint || !urdf_joint->safety) - { - return false; - } - - soft_limits.min_position = urdf_joint->safety->soft_lower_limit; - soft_limits.max_position = urdf_joint->safety->soft_upper_limit; - soft_limits.k_position = urdf_joint->safety->k_position; - soft_limits.k_velocity = urdf_joint->safety->k_velocity; - - return true; -} - -} // namespace joint_limits_interface - -#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_URDF_HPP_ diff --git a/joint_limits_interface/mainpage.dox b/joint_limits_interface/mainpage.dox deleted file mode 100644 index 9b23463bc3..0000000000 --- a/joint_limits_interface/mainpage.dox +++ /dev/null @@ -1,142 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -joint_limits_interface contains data structures for representing joint limits, methods for -populating them from common formats such as URDF and rosparam, and methods for enforcing limits on different kinds -of joint commands. - -The joint_limits_interface is \e not used by controllers themselves (it does not implement a \p HardwareInterface) but instead operates after the controllers have updated, in the \p write() method (or equivalent) of the robot abstraction. Enforcing limits will \e overwrite the commands set by the controllers, it does not operate on a separate raw data buffer. - -\section codeapi Code API - -There are two main elements involved in setting up a joint_limits_interface: - -\subsection limits_representation Joint limits representation - - - \ref joint_limits_interface::JointLimits "JointLimits" Position, velocity, acceleration, jerk and effort. - - \ref joint_limits_interface::SoftJointLimits "SoftJointLimits" Soft position limits, k_p, k_v (as described here ). - - \ref joint_limits_urdf.h "Convenience methods" for loading joint limits information (only position, velocity, effort), as well as soft joint limits information from the URDF. - - \ref joint_limits_rosparam.h "Convenience methods" for loading joint limits from ROS parameter server (all values). Parameter specification is the same used in MoveIt, with the addition that we also parse jerk and effort limits. - -\subsection limits_interface Joint limits interface - -For \b effort-controlled joints, \b position-controlled joints, and \b velocity-controlled joints, two types of interfaces have been created. The first is a saturation interface, used for joints that have normal limits but not soft limits. The second is an interface that implements soft limits, similar to the one used on the PR2. - - - Effort-controlled joints - - Saturation: \ref joint_limits_interface::EffortJointSaturationHandle "Handle", \ref joint_limits_interface::EffortJointSaturationInterface "Interface" - - Soft limits: \ref joint_limits_interface::EffortJointSoftLimitsHandle "Handle", \ref joint_limits_interface::EffortJointSoftLimitsInterface "Interface" - - Position-controlled joints - - Saturation: \ref joint_limits_interface::PositionJointSaturationHandle "Handle", \ref joint_limits_interface::PositionJointSaturationInterface "Interface" - - Soft limits: \ref joint_limits_interface::PositionJointSoftLimitsHandle "Handle", \ref joint_limits_interface::PositionJointSoftLimitsInterface "Interface" - - Velocity-controlled joints - - Saturation: \ref joint_limits_interface::VelocityJointSaturationHandle "Handle", \ref joint_limits_interface::VelocityJointSaturationInterface "interface" - - Soft limits: \ref joint_limits_interface::VelocityJointSoftLimitsHandle "Handle", \ref joint_limits_interface::VelocityJointSoftLimitsInterface "Interface" - -\section example Examples - -\subsection limits_representation_example Joint limits representation - -The first example shows the different ways of populating joint limits data structures. - -\code -#include - -#include -#include -#include - -int main(int argc, char** argv) -{ - // Init node handle and URDF model - ros::NodeHandle nh; - boost::shared_ptr urdf; - // ...initialize contents of urdf - - // Data structures - joint_limits_interface::JointLimits limits; - joint_limits_interface::SoftJointLimits soft_limits; - - // Manual value setting - limits.has_velocity_limits = true; - limits.max_velocity = 2.0; - - // Populate (soft) joint limits from URDF - // Limits specified in URDF overwrite existing values in 'limits' and 'soft_limits' - // Limits not specified in URDF preserve their existing values - urdf::JointConstSharedPtr urdf_joint = urdf->getJoint("foo_joint"); - const bool urdf_limits_ok = getJointLimits(urdf_joint, limits); - const bool urdf_soft_limits_ok = getSoftJointLimits(urdf_joint, soft_limits); - - // Populate (soft) joint limits from the ros parameter server - // Limits specified in the parameter server overwrite existing values in 'limits' and 'soft_limits' - // Limits not specified in the parameter server preserve their existing values - const bool rosparam_limits_ok = getJointLimits("foo_joint", nh, limits); -} -\endcode - -A joint limits specification in YAML format that can be loaded to the ROS parameter server can be found -\ref joint_limits_interface::getJointLimits(const std::string& joint_name, const ros::NodeHandle& nh, JointLimits& limits) "here". - -\subsection limits_interface_example Joint limits interface - -The second example integrates joint limits enforcing into an existing robot hardware implementation. - -\code -#include - -using namespace hardware_interface; -using joint_limits_interface::JointLimits; -using joint_limits_interface::SoftJointLimits; -using joint_limits_interface::PositionJointSoftLimitsHandle; -using joint_limits_interface::PositionJointSoftLimitsInterface; - -class MyRobot -{ -public: - MyRobot() {} - - bool init() - { - // Populate pos_cmd_interface_ with joint handles... - - // Get joint handle of interest - JointHandle joint_handle = pos_cmd_interface_.getHandle("foo_joint"); - - JointLimits limits; - SoftJointLimits soft_limits; - // Populate with any of the methods presented in the previous example... - - // Register handle in joint limits interface - PositionJointSoftLimitsHandle handle(joint_handle, // We read the state and read/write the command - limits, // Limits spec - soft_limits) // Soft limits spec - - jnt_limits_interface_.registerHandle(handle); - } - - void read(ros::Time time, ros::Duration period) - { - // Read actuator state from hardware... - - // Propagate current actuator state to joints... - } - - void write(ros::Time time, ros::Duration period) - { - // Enforce joint limits for all registered handles - // Note: one can also enforce limits on a per-handle basis: handle.enforce_limits(period) - jnt_limits_interface_.enforce_limits(period); - - // Propagate joint commands to actuators... - - // Send actuator command to hardware... - } - -private: - PositionJointInterface pos_cmd_interface_; - PositionJointSoftLimitsInterface jnt_limits_interface_; -}; -\endcode - -*/ diff --git a/joint_limits_interface/package.xml b/joint_limits_interface/package.xml deleted file mode 100644 index badbb51773..0000000000 --- a/joint_limits_interface/package.xml +++ /dev/null @@ -1,36 +0,0 @@ - - joint_limits_interface - 0.0.0 - Interface for enforcing joint limits. - - Bence Magyar - Denis Štogl - - 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 - - Adolfo Rodriguez Tsouroukdissian - - ament_cmake - - rclcpp - urdf - - hardware_interface - - hardware_interface - - ament_cmake_gtest - hardware_interface - launch - launch_ros - launch_testing - launch_testing_ament_cmake - - - ament_cmake - - diff --git a/joint_limits_interface/test/joint_limits_urdf_test.cpp b/joint_limits_interface/test/joint_limits_urdf_test.cpp deleted file mode 100644 index 55effc7117..0000000000 --- a/joint_limits_interface/test/joint_limits_urdf_test.cpp +++ /dev/null @@ -1,176 +0,0 @@ -// Copyright 2020 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 Adolfo Rodriguez Tsouroukdissian - -#include - -#include - -#include - -class JointLimitsUrdfTest : public ::testing::Test -{ -public: - JointLimitsUrdfTest() - { - urdf_limits.reset(new urdf::JointLimits); - urdf_limits->effort = 8.0; - urdf_limits->velocity = 2.0; - urdf_limits->lower = -1.0; - urdf_limits->upper = 1.0; - - urdf_safety.reset(new urdf::JointSafety); - urdf_safety->k_position = 20.0; - urdf_safety->k_velocity = 40.0; - urdf_safety->soft_lower_limit = -0.8; - urdf_safety->soft_upper_limit = 0.8; - - urdf_joint.reset(new urdf::Joint); - urdf_joint->limits = urdf_limits; - urdf_joint->safety = urdf_safety; - - urdf_joint->type = urdf::Joint::UNKNOWN; - } - -protected: - urdf::JointLimitsSharedPtr urdf_limits; - urdf::JointSafetySharedPtr urdf_safety; - urdf::JointSharedPtr urdf_joint; -}; - -TEST_F(JointLimitsUrdfTest, GetJointLimits) -{ - // Unset URDF joint - { - joint_limits_interface::JointLimits limits; - urdf::JointSharedPtr urdf_joint_bad; - EXPECT_FALSE(getJointLimits(urdf_joint_bad, limits)); - } - - // Unset URDF limits - { - joint_limits_interface::JointLimits limits; - urdf::JointSharedPtr urdf_joint_bad(new urdf::Joint); - EXPECT_FALSE(getJointLimits(urdf_joint_bad, limits)); - } - - // Valid URDF joint, CONTINUOUS type - { - urdf_joint->type = urdf::Joint::CONTINUOUS; - - joint_limits_interface::JointLimits limits; - EXPECT_TRUE(getJointLimits(urdf_joint, limits)); - - // Position - EXPECT_FALSE(limits.has_position_limits); - EXPECT_TRUE(limits.angle_wraparound); - - // Velocity - EXPECT_TRUE(limits.has_velocity_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->velocity, limits.max_velocity); - - // Acceleration - EXPECT_FALSE(limits.has_acceleration_limits); - - // Effort - EXPECT_TRUE(limits.has_effort_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->effort, limits.max_effort); - } - - // Valid URDF joint, REVOLUTE type - { - urdf_joint->type = urdf::Joint::REVOLUTE; - - joint_limits_interface::JointLimits limits; - EXPECT_TRUE(getJointLimits(urdf_joint, limits)); - - // Position - EXPECT_TRUE(limits.has_position_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->lower, limits.min_position); - EXPECT_DOUBLE_EQ(urdf_joint->limits->upper, limits.max_position); - EXPECT_FALSE(limits.angle_wraparound); - - // Velocity - EXPECT_TRUE(limits.has_velocity_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->velocity, limits.max_velocity); - - // Acceleration - EXPECT_FALSE(limits.has_acceleration_limits); - - // Effort - EXPECT_TRUE(limits.has_effort_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->effort, limits.max_effort); - } - - // Valid URDF joint, PRISMATIC type - { - urdf_joint->type = urdf::Joint::PRISMATIC; - - joint_limits_interface::JointLimits limits; - EXPECT_TRUE(getJointLimits(urdf_joint, limits)); - - // Position - EXPECT_TRUE(limits.has_position_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->lower, limits.min_position); - EXPECT_DOUBLE_EQ(urdf_joint->limits->upper, limits.max_position); - EXPECT_FALSE(limits.angle_wraparound); - - // Velocity - EXPECT_TRUE(limits.has_velocity_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->velocity, limits.max_velocity); - - // Acceleration - EXPECT_FALSE(limits.has_acceleration_limits); - - // Effort - EXPECT_TRUE(limits.has_effort_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->effort, limits.max_effort); - } -} - -TEST_F(JointLimitsUrdfTest, GetSoftJointLimits) -{ - // Unset URDF joint - { - joint_limits_interface::SoftJointLimits soft_limits; - urdf::JointSharedPtr urdf_joint_bad; - EXPECT_FALSE(getSoftJointLimits(urdf_joint_bad, soft_limits)); - } - - // Unset URDF limits - { - joint_limits_interface::SoftJointLimits soft_limits; - urdf::JointSharedPtr urdf_joint_bad(new urdf::Joint); - EXPECT_FALSE(getSoftJointLimits(urdf_joint_bad, soft_limits)); - } - - // Valid URDF joint - { - joint_limits_interface::SoftJointLimits soft_limits; - EXPECT_TRUE(getSoftJointLimits(urdf_joint, soft_limits)); - - // Soft limits - EXPECT_DOUBLE_EQ(urdf_joint->safety->soft_lower_limit, soft_limits.min_position); - EXPECT_DOUBLE_EQ(urdf_joint->safety->soft_upper_limit, soft_limits.max_position); - EXPECT_DOUBLE_EQ(urdf_joint->safety->k_position, soft_limits.k_position); - EXPECT_DOUBLE_EQ(urdf_joint->safety->k_velocity, soft_limits.k_velocity); - } -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -}