diff --git a/joint_limits_interface/CHANGELOG.rst b/joint_limits_interface/CHANGELOG.rst new file mode 100644 index 0000000000..7efad9e9f2 --- /dev/null +++ b/joint_limits_interface/CHANGELOG.rst @@ -0,0 +1,247 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package joint_limits_interface +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.16.0 (2020-01-27) +------------------- +* Merge pull request `#413 `_ from matthew-reynolds/range-for + Use range-based for loop +* Use more meaningful pair iterator names +* Merge pull request `#404 `_ from matthew-reynolds/catkin-lint + Update CMakeLists.txt and package.xml +* Use range-based for loops in joint_limits_interface +* Update dependencies + - Dependencies needed to compile are + - Dependencies used in public headers are + - Dependencies needed to link or run are +* Merge branch 'melodic-devel' into catkin-lint +* Update package dependencies +* Remove liburdfdom-dev +* Add missing roscpp & rospy dependencies +* Remove rosunit test_depend from package.xml +* Merge pull request `#405 `_ from matthew-reynolds/use-nullptr + Use nullptr +* Prefer 0.0 for floating point literals +* Merge pull request `#406 `_ from matthew-reynolds/pragma-once + Use #pragma once +* Replace header guard with #pragma once +* Merge pull request `#395 `_ from pal-robotics-forks/extend-interfaces-melodic + Extend interfaces +* joint_limits: use an open-loop policy for velocity staturation + The feedback from the controller is way too slow to be used on an + actual robot. A robot that had 15 rad.s^-2 on each wheel as + an acceleration limit could not even reach 2 rad.s^-2 + This is in line with ros_controllers`#23 `_ +* Apply consistent style to CMakeLists.txt files +* Apply consistent style to package.xml files +* Merge pull request `#398 `_ from matthew-reynolds/revert-cmake + Revert CMake include_directories as SYSTEM +* Revert CMake include_directories as SYSTEM +* Merge pull request `#396 `_ from pal-robotics-forks/small-fixes + Small fixes +* Fix shadowed variables +* -Werror=overloaded-virtual and initialization of fields in constructor +* Contributors: Bence Magyar, Daniel Pinyol, Matt Reynolds, Paul Mathieu, Victor Lopez + +0.15.1 (2018-09-30) +------------------- + +0.15.0 (2018-05-28) +------------------- + +0.14.2 (2018-04-26) +------------------- +* Update maintainers +* Fix catkin_lint errors and warnings +* Contributors: Bence Magyar + +0.14.1 (2018-04-16) +------------------- + +0.14.0 (2018-03-26) +------------------- + +0.13.0 (2017-12-23) +------------------- +* Add method to populate SoftJointLimits from ROS parameter server. (`#292 `_) +* Contributors: Miguel Prada + +0.12.0 (2017-08-05) +------------------- + +0.11.5 (2017-06-28) +------------------- +* Throw error if EffortJointSaturationHandle is missing effort or velocity limits +* Contributors: Bence Magyar, Dave Coleman + +0.11.4 (2017-02-14) +------------------- + +0.11.3 (2016-12-07) +------------------- +* Add urdf compatibility header +* Contributors: Bence Magyar + +0.11.2 (2016-11-28) +------------------- +* Add Enrique and Bence to maintainer list +* Clean up export leftovers from rosbuild +* Convert to format2, fix dependency in cmake +* Replace boost::shared_ptr with urdf::XYConstSharedPtr +* Contributors: Bence Magyar + +0.11.1 (2016-08-18) +------------------- + +0.11.0 (2016-05-23) +------------------- + +0.10.1 (2016-04-23) +------------------- +* Fix catkin_package + * Don't export local include dirs. + * Fix dependency on urdfdom (Thanks to Mathias Lüdtke). +* Contributors: Jochen Sprickerhof + +0.10.0 (2015-11-20) +------------------- + +0.9.3 (2015-05-05) +------------------ + +0.9.2 (2015-05-04) +------------------ +* Reset functionality for stateful position joint limit handles +* Contributors: Mathias Lüdtke + +0.9.1 (2014-11-03) +------------------ + +0.9.0 (2014-10-31) +------------------ +* Buildsystem and documentation fixes +* Add inline keyword to free header functions +* Contributors: Adolfo Rodriguez Tsouroukdissian, Lukas Bulwahn, shadowmanos + +0.8.2 (2014-06-25) +------------------ +* Propagate urdfdom changes to CMakeLists.txt + urdfdom is now standalone, so it must be find_package'd independently. +* Fix rostest, which was not being built correctly. +* Contributors: Adolfo Rodriguez Tsouroukdissian + +0.8.1 (2014-06-24) +------------------ +* Use upstream liburdfdom-dev package. + Refs `ros/rosdistro#4633 `_. +* Contributors: Adolfo Rodriguez Tsouroukdissian + +0.8.0 (2014-05-12) +------------------ +* Remove rosbuild artifacts. Fix `#154 `_. +* Contributors: Adolfo Rodriguez Tsouroukdissian + +0.7.2 (2014-04-01) +------------------ + +0.7.1 (2014-03-31) +------------------ +* Fix dependency specification in CMake script to allow isolated builds. +* Contributors: Adolfo Rodriguez Tsouroukdissian + +0.7.0 (2014-03-28) +------------------ + +0.6.0 (2014-02-05) +------------------ +* Updated the interface list. +* Added the PositionJointSaturationInterface and VelocitySoftLimitsInterface + classes. There are now saturation and soft limit classes for effort-controlled, + position-controlled, and velocity-controlled joints. +* Contributors: Jim Rothrock + +0.5.8 (2013-10-11) +------------------ +* Merge pull request `#121 `_ from pal-robotics/hydro-devel + Fixes for next minor release +* Added the EffortJointSaturationHandle and EffortJointSaturationInterface + classes. They are used with joints that do not have soft limits specified in + their URDF files. +* Minor documentation precision. +* Make position joint limits handle opn loop. + - Lowers the entry barrier for simple robots without velocity measurements, + poor control tracking or with a slow update rate. +* Update README.md +* Create README.md +* CMakeLists fix to fit with OpenEmbedded/Yocto meta-ros layer. + Increase the compatibility of the ros_control code with + meta-ros, an OpenEmbedded/Yocto layer that provides recipes for ROS + packages disabling catking checking the variable CATKIN_ENABLE_TESTING. +* Fix license header in some files. +* Renamed joint_limits_interface manifext.xml + +0.5.7 (2013-07-30) +------------------ + +* Updated changelogs +* Add angle_wraparound joint limit property. + For full compatibility with MoveIt!'s joint limit specification. + Note that we still have the extra effort and jerk specification. + +0.5.6 (2013-07-29) +------------------ + +0.5.5 (2013-07-23) +------------------ + +0.5.4 (2013-07-23) +------------------ + +0.5.3 (2013-07-22) +------------------ + +0.5.2 (2013-07-22) +------------------ +* Fixed gtests for joint_limits_interface in catkin +* Merge pull request `#93 `_ from pal-robotics/master + joint_limits_interface broken in Groocy and Hydro +* Fix for joint_limits tests in catkin +* Restore urdf dependencies. + Add conditional compilation for Fuerte and Groovy+ distros. + +0.5.1 (2013-07-19) +------------------ + +0.5.0 (2013-07-16) +------------------ +* Made joint_limits_interface match hydro version number +* Removed urdf_interface dependencies +* Add meta tags to packages not specifying them. + - Website, bugtracker, repository. +* Better documentation of YAML joint limits spec. + - Add cross-references in doc main page. +* Documentation improvements. + - More consistency between transmission and joint limits interfaces doc. + - Make explicit that these interfaces are not meant to be used by controllers, + but by the robot abstraction. +* build dependency rostest added to package.xml and rostest added to CMakeLists.txt +* Added dependency for rostest to fix build error +* Fix compiler warnings (-Wreorder) +* Minor doc structure improvements. +* Add main page to joint_limits_interface doc. +* Remove temporary file from version control. +* Add attribution for soft_limits code. + - Soft-limits enforcing is based on a previous implementation by Willow Garage. + Add them in the copyright holders list. +* Lower severity of log message. +* Allow unsetting limits specification from rosparam. + - Update tests. +* Add .gitignore +* Add joint limits parsing from rosparam + unit test. +* Add max_jerk to limits specification. +* Minor maintenance fixes. +* Add documentation. +* Extensive file, namespace, class renaming. + +0.4.0 (2013-06-25) +------------------ diff --git a/joint_limits_interface/CMakeLists.txt b/joint_limits_interface/CMakeLists.txt new file mode 100644 index 0000000000..f51303354a --- /dev/null +++ b/joint_limits_interface/CMakeLists.txt @@ -0,0 +1,50 @@ +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(ament_lint_auto REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) + + ament_lint_auto_find_test_dependencies() + + 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) +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/README.md b/joint_limits_interface/README.md new file mode 100644 index 0000000000..c77ccc2b75 --- /dev/null +++ b/joint_limits_interface/README.md @@ -0,0 +1,30 @@ +## 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.hpp b/joint_limits_interface/include/joint_limits_interface/joint_limits.hpp new file mode 100644 index 0000000000..7795e135e6 --- /dev/null +++ b/joint_limits_interface/include/joint_limits_interface/joint_limits.hpp @@ -0,0 +1,90 @@ +// Copyright 2013, PAL Robotics S.L. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the copyright holders nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_HPP_ +#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_HPP_ + +namespace joint_limits_interface +{ + +struct JointLimits +{ + JointLimits() + : min_position(0.0), + max_position(0.0), + max_velocity(0.0), + max_acceleration(0.0), + max_jerk(0.0), + max_effort(0.0), + has_position_limits(false), + has_velocity_limits(false), + has_acceleration_limits(false), + has_jerk_limits(false), + has_effort_limits(false), + angle_wraparound(false) + {} + + double min_position; + double max_position; + double max_velocity; + double max_acceleration; + double max_jerk; + double max_effort; + + bool has_position_limits; + bool has_velocity_limits; + bool has_acceleration_limits; + bool has_jerk_limits; + bool has_effort_limits; + bool angle_wraparound; +}; + +struct SoftJointLimits +{ + SoftJointLimits() + : min_position(0.0), + max_position(0.0), + k_position(0.0), + k_velocity(0.0) + {} + + double min_position; + double max_position; + double k_position; + double k_velocity; +}; + +} // namespace joint_limits_interface + +#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_HPP_ 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 new file mode 100644 index 0000000000..7f5e52f184 --- /dev/null +++ b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp @@ -0,0 +1,664 @@ +// Copyright 2013, PAL Robotics S.L. +// Copyright 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the copyright holders nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/// \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 +#include +#include +#include + +#include "joint_limits_interface/joint_limits.hpp" +#include "joint_limits_interface/joint_limits_interface_exception.hpp" + + +namespace joint_limits_interface +{ + +/** \brief A handle used to enforce position and velocity limits of a position-controlled joint that does not have + soft limits. */ +class PositionJointSaturationHandle +{ +public: + PositionJointSaturationHandle() {} + + PositionJointSaturationHandle( + const hardware_interface::JointStateHandle & jh, + const hardware_interface::JointCommandHandle & jcmdh, + const joint_limits_interface::JointLimits & limits) + : jh_(jh), + jcmdh_(jcmdh), + limits_(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(); + } + + prev_cmd_ = std::numeric_limits::quiet_NaN(); + } + + /** \return Joint name. */ + std::string get_name() const {return jh_.get_name();} + + /** + * \brief Enforce position and velocity limits for a joint that is not subject to soft limits. + * + * \param period Control period. + */ + void enforceLimits(const rclcpp::Duration & period) + { + if (std::isnan(prev_cmd_)) { + prev_cmd_ = jh_.get_position(); + } + + double min_pos, max_pos; + if (limits_.has_velocity_limits) { + const double delta_pos = limits_.max_velocity * period.seconds(); + min_pos = std::max(prev_cmd_ - delta_pos, min_pos_limit_); + max_pos = std::min(prev_cmd_ + delta_pos, max_pos_limit_); + } else { + min_pos = min_pos_limit_; + max_pos = max_pos_limit_; + } + + const double cmd = rcppmath::clamp(jcmdh_.get_cmd(), min_pos, max_pos); + jcmdh_.set_cmd(cmd); + + prev_cmd_ = cmd; + } + + /** + * \brief Reset state, in case of mode switch or e-stop + */ + void reset() + { + prev_cmd_ = std::numeric_limits::quiet_NaN(); + } + +private: + hardware_interface::JointStateHandle jh_; + hardware_interface::JointCommandHandle jcmdh_; + joint_limits_interface::JointLimits limits_; + double min_pos_limit_, max_pos_limit_; + double prev_cmd_; +}; + +/** + * \brief 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: + PositionJointSoftLimitsHandle() + : prev_cmd_(std::numeric_limits::quiet_NaN()) + {} + + PositionJointSoftLimitsHandle( + const hardware_interface::JointStateHandle & jh, + const hardware_interface::JointCommandHandle & jcmdh, + const joint_limits_interface::JointLimits & limits, + const joint_limits_interface::SoftJointLimits & soft_limits) + : jh_(jh), + jcmdh_(jcmdh), + limits_(limits), + soft_limits_(soft_limits), + prev_cmd_(std::numeric_limits::quiet_NaN()) + { + if (!limits.has_velocity_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no velocity limits specification."); + } + } + + /** \return Joint name. */ + std::string get_name() const {return jh_.get_name();} + + /** + * \brief 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 period Control period. + */ + void enforceLimits(const rclcpp::Duration & period) + { + assert(period.seconds() > 0.0); + + // Current position + // TODO(anyone): Doc! + if (std::isnan(prev_cmd_)) { + // Happens only once at initialization + prev_cmd_ = jh_.get_position(); + } + const double pos = prev_cmd_; + + // 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 = rcppmath::clamp( + -soft_limits_.k_position * (pos - soft_limits_.min_position), + -limits_.max_velocity, + limits_.max_velocity); + + soft_max_vel = rcppmath::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 = rcppmath::clamp( + jcmdh_.get_cmd(), + pos_low, + pos_high); + jcmdh_.set_cmd(pos_cmd); + + // Cache variables + prev_cmd_ = jcmdh_.get_cmd(); + } + + /** + * \brief Reset state, in case of mode switch or e-stop + */ + void reset() + { + prev_cmd_ = std::numeric_limits::quiet_NaN(); + } + +private: + hardware_interface::JointStateHandle jh_; + hardware_interface::JointCommandHandle jcmdh_; + joint_limits_interface::JointLimits limits_; + joint_limits_interface::SoftJointLimits soft_limits_; + + double prev_cmd_; +}; + +/** \brief A handle used to enforce position, velocity, and effort limits of an effort-controlled + * joint that does not have soft limits. + */ +class EffortJointSaturationHandle +{ +public: + EffortJointSaturationHandle() {} + + EffortJointSaturationHandle( + const hardware_interface::JointStateHandle & jh, + const hardware_interface::JointCommandHandle & jcmdh, + const joint_limits_interface::JointLimits & limits) + : jh_(jh), + jcmdh_(jcmdh), + limits_(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."); + } + } + + /** \return Joint name. */ + std::string get_name() const {return jh_.get_name();} + + /** + * \brief Enforce position, velocity, and effort limits for a joint that is not subject + * to soft limits. + */ + void enforceLimits(const rclcpp::Duration & /* period */) + { + double min_eff = -limits_.max_effort; + double max_eff = limits_.max_effort; + + if (limits_.has_position_limits) { + const double pos = jh_.get_position(); + if (pos < limits_.min_position) { + min_eff = 0.0; + } else if (pos > limits_.max_position) { + max_eff = 0.0; + } + } + + const double vel = jh_.get_velocity(); + if (vel < -limits_.max_velocity) { + min_eff = 0.0; + } else if (vel > limits_.max_velocity) { + max_eff = 0.0; + } + + double clamped = rcppmath::clamp(jcmdh_.get_cmd(), min_eff, max_eff); + jcmdh_.set_cmd(clamped); + } + +private: + hardware_interface::JointStateHandle jh_; + hardware_interface::JointCommandHandle jcmdh_; + joint_limits_interface::JointLimits limits_; +}; + +/** \brief 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: + EffortJointSoftLimitsHandle() {} + + EffortJointSoftLimitsHandle( + const hardware_interface::JointStateHandle & jh, + const hardware_interface::JointCommandHandle & jcmdh, + const joint_limits_interface::JointLimits & limits, + const joint_limits_interface::SoftJointLimits & soft_limits) + : jh_(jh), + jcmdh_(jcmdh), + limits_(limits), + soft_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."); + } + } + + /** \return Joint name. */ + std::string get_name() const {return jh_.get_name();} + + /** + * \brief 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. + */ + void enforceLimits(const rclcpp::Duration & /*period*/) + { + // Current state + const double pos = jh_.get_position(); + const double vel = jh_.get_velocity(); + + // 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 = rcppmath::clamp( + -soft_limits_.k_position * (pos - soft_limits_.min_position), + -limits_.max_velocity, + limits_.max_velocity); + + soft_max_vel = rcppmath::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 = rcppmath::clamp( + -soft_limits_.k_velocity * (vel - soft_min_vel), + -limits_.max_effort, + limits_.max_effort); + + const double soft_max_eff = rcppmath::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 = rcppmath::clamp( + jcmdh_.get_cmd(), + soft_min_eff, + soft_max_eff); + jcmdh_.set_cmd(eff_cmd); + } + +private: + hardware_interface::JointStateHandle jh_; + hardware_interface::JointCommandHandle jcmdh_; + joint_limits_interface::JointLimits limits_; + joint_limits_interface::SoftJointLimits soft_limits_; +}; + + +/** \brief A handle used to enforce velocity and acceleration limits of a velocity-controlled joint. + */ +class VelocityJointSaturationHandle +{ +public: + VelocityJointSaturationHandle() + : prev_cmd_(0.0) + {} + + VelocityJointSaturationHandle( + const hardware_interface::JointStateHandle & jh, + const hardware_interface::JointCommandHandle & jcmdh, + const joint_limits_interface::JointLimits & limits) + : jh_(jh), + jcmdh_(jcmdh), + limits_(limits), + prev_cmd_(0.0) + { + if (!limits.has_velocity_limits) { + throw joint_limits_interface::JointLimitsInterfaceException( + "Cannot enforce limits for joint '" + get_name() + + "'. It has no velocity limits specification."); + } + } + + /** \return Joint name. */ + std::string get_name() const {return jh_.get_name();} + + /** + * \brief Enforce joint velocity and acceleration limits. + * \param period Control period. + */ + void enforceLimits(const rclcpp::Duration & period) + { + // 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_cmd_ - limits_.max_acceleration * dt, -limits_.max_velocity); + vel_high = std::min(prev_cmd_ + 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 = rcppmath::clamp( + jcmdh_.get_cmd(), + vel_low, + vel_high); + jcmdh_.set_cmd(vel_cmd); + + // Cache variables + prev_cmd_ = jcmdh_.get_cmd(); + } + +private: + hardware_interface::JointStateHandle jh_; + hardware_interface::JointCommandHandle jcmdh_; + joint_limits_interface::JointLimits limits_; + + double prev_cmd_; +}; + +/** \brief A handle used to enforce position, velocity, and acceleration limits of a + * velocity-controlled joint. + */ +class VelocityJointSoftLimitsHandle +{ +public: + VelocityJointSoftLimitsHandle() {} + VelocityJointSoftLimitsHandle( + const hardware_interface::JointStateHandle & jh, + const hardware_interface::JointCommandHandle & jcmdh, + const joint_limits_interface::JointLimits & limits, + const joint_limits_interface::SoftJointLimits & soft_limits) + { + jh_ = jh; + jcmdh_ = jcmdh; + limits_ = limits; + soft_limits_ = soft_limits; + if (limits.has_velocity_limits) { + max_vel_limit_ = limits.max_velocity; + } else { + max_vel_limit_ = std::numeric_limits::max(); + } + } + + /** \return Joint name. */ + std::string get_name() const {return jh_.get_name();} + + /** + * \brief Enforce position, velocity, and acceleration limits for a velocity-controlled joint + * subject to soft limits. + * + * \param period Control period. + */ + void enforceLimits(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 = jh_.get_position(); + min_vel = rcppmath::clamp( + -soft_limits_.k_position * (pos - soft_limits_.min_position), + -max_vel_limit_, max_vel_limit_); + max_vel = rcppmath::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 = jh_.get_velocity(); + 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_cmd(rcppmath::clamp(jcmdh_.get_cmd(), min_vel, max_vel)); + } + +private: + hardware_interface::JointStateHandle jh_; + hardware_interface::JointCommandHandle jcmdh_; + joint_limits_interface::JointLimits limits_; + joint_limits_interface::SoftJointLimits soft_limits_; + double max_vel_limit_; +}; + +// TODO(anyone): Port this to ROS 2 +// //** +// * \brief Interface for enforcing joint limits. +// * +// * \tparam HandleType %Handle type. Must implement the following methods: +// * \code +// * void enforceLimits(); +// * 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 +// *\{*/ +// /** \brief Enforce limits for all managed handles. */ +// void enforceLimits(const rclcpp::Duration & period) +// { +// for (auto && resource_name_and_handle : this->resource_map_) { +// resource_name_and_handle.second.enforceLimits(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 +// *\{*/ +// /** \brief 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 +// *\{*/ +// /** \brief 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 new file mode 100644 index 0000000000..4f0acaf47d --- /dev/null +++ b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface_exception.hpp @@ -0,0 +1,61 @@ +// Copyright 2013, PAL Robotics S.L. All rights reserved. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the copyright holders nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#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) {} + + virtual ~JointLimitsInterfaceException() throw() {} + + virtual const char * what() const throw() + { + 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_rosparam.hpp b/joint_limits_interface/include/joint_limits_interface/joint_limits_rosparam.hpp new file mode 100644 index 0000000000..09b1fe8bef --- /dev/null +++ b/joint_limits_interface/include/joint_limits_interface/joint_limits_rosparam.hpp @@ -0,0 +1,256 @@ +// Copyright 2013, PAL Robotics S.L. All rights reserved. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the copyright holders nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_ROSPARAM_HPP_ +#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_ROSPARAM_HPP_ + +#include + +#include + +#include + +namespace joint_limits_interface +{ + +/** + * \brief Populate a JointLimits instance from the ROS parameter server. + * + * It is assumed that the following parameter structure is followed on the provided NodeHandle. Unspecified parameters + * are simply not added to the joint limits specification. + * \code + * joint_limits: + * foo_joint: + * has_position_limits: true + * min_position: 0.0 + * max_position: 1.0 + * has_velocity_limits: true + * max_velocity: 2.0 + * has_acceleration_limits: true + * max_acceleration: 5.0 + * has_jerk_limits: true + * max_jerk: 100.0 + * has_effort_limits: true + * max_effort: 20.0 + * bar_joint: + * has_position_limits: false # Continuous joint + * has_velocity_limits: true + * max_velocity: 4.0 + * \endcode + * + * This specification is similar to the one used by MoveIt!, + * but additionally supports jerk and effort limits. + * + * \param[in] joint_name Name of joint whose limits are to be fetched. + * \param[in] node NodeHandle where the joint limits are specified. + * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite + * existing values. Values in \p limits not specified in the parameter server remain unchanged. + * \return True if a limits specification is found (ie. the \p joint_limits/joint_name parameter exists in \p node), false otherwise. + */ +inline bool getJointLimits( + const std::string & joint_name, const rclcpp::Node::SharedPtr & node, + JointLimits & limits) +{ + const std::string param_base_name = "joint_limits." + joint_name; + try { + if (!node->has_parameter(param_base_name + ".has_position_limits") && + !node->has_parameter(param_base_name + ".min_position") && + !node->has_parameter(param_base_name + ".max_position") && + !node->has_parameter(param_base_name + ".has_velocity_limits") && + !node->has_parameter(param_base_name + ".min_velocity") && + !node->has_parameter(param_base_name + ".max_velocity") && + !node->has_parameter(param_base_name + ".has_acceleration_limits") && + !node->has_parameter(param_base_name + ".max_acceleration") && + !node->has_parameter(param_base_name + ".has_jerk_limits") && + !node->has_parameter(param_base_name + ".max_jerk") && + !node->has_parameter(param_base_name + ".has_effort_limits") && + !node->has_parameter(param_base_name + ".max_effort") && + !node->has_parameter(param_base_name + ".angle_wraparound") && + !node->has_parameter(param_base_name + ".has_soft_limits") && + !node->has_parameter(param_base_name + ".k_position") && + !node->has_parameter(param_base_name + ".k_velocity") && + !node->has_parameter(param_base_name + ".soft_lower_limit") && + !node->has_parameter(param_base_name + ".soft_upper_limit")) + { + RCLCPP_ERROR_STREAM( + node->get_logger(), "No joint limits specification found for joint '" << joint_name << + "' in the parameter server (node: " << std::string( + node->get_name()) + " param name: " + param_base_name << ")."); + return false; + } + } catch (const std::exception & ex) { + RCLCPP_ERROR_STREAM(node->get_logger(), ex.what()); + return false; + } + + // Position limits + bool has_position_limits = false; + if (node->get_parameter(param_base_name + ".has_position_limits", has_position_limits)) { + if (!has_position_limits) {limits.has_position_limits = false;} + double min_pos, max_pos; + if (has_position_limits && + node->get_parameter(param_base_name + ".min_position", min_pos) && + node->get_parameter(param_base_name + ".max_position", max_pos)) + { + limits.has_position_limits = true; + limits.min_position = min_pos; + limits.max_position = max_pos; + } + + bool angle_wraparound; + if (!has_position_limits && + node->get_parameter(param_base_name + ".angle_wraparound", angle_wraparound)) + { + limits.angle_wraparound = angle_wraparound; + } + } + + // Velocity limits + bool has_velocity_limits = false; + if (node->get_parameter(param_base_name + ".has_velocity_limits", has_velocity_limits)) { + if (!has_velocity_limits) {limits.has_velocity_limits = false;} + double max_vel; + if (has_velocity_limits && node->get_parameter(param_base_name + ".max_velocity", max_vel)) { + limits.has_velocity_limits = true; + limits.max_velocity = max_vel; + } + } + + // Acceleration limits + bool has_acceleration_limits = false; + if (node->get_parameter(param_base_name + ".has_acceleration_limits", has_acceleration_limits)) { + if (!has_acceleration_limits) {limits.has_acceleration_limits = false;} + double max_acc; + if (has_acceleration_limits && + node->get_parameter(param_base_name + ".max_acceleration", max_acc)) + { + limits.has_acceleration_limits = true; + limits.max_acceleration = max_acc; + } + } + + // Jerk limits + bool has_jerk_limits = false; + if (node->get_parameter(param_base_name + ".has_jerk_limits", has_jerk_limits)) { + if (!has_jerk_limits) {limits.has_jerk_limits = false;} + double max_jerk; + if (has_jerk_limits && node->get_parameter(param_base_name + ".max_jerk", max_jerk)) { + limits.has_jerk_limits = true; + limits.max_jerk = max_jerk; + } + } + + // Effort limits + bool has_effort_limits = false; + if (node->get_parameter(param_base_name + ".has_effort_limits", has_effort_limits)) { + if (!has_effort_limits) {limits.has_effort_limits = false;} + double max_effort; + if (has_effort_limits && node->get_parameter(param_base_name + ".max_effort", max_effort)) { + limits.has_effort_limits = true; + limits.max_effort = max_effort; + } + } + + return true; +} + +/** + * \brief Populate a SoftJointLimits instance from the ROS parameter server. + * + * It is assumed that the following parameter structure is followed on the provided NodeHandle. Only completely specified soft + * joint limits specifications will be considered valid. + * \code + * joint_limits: + * foo_joint: + * soft_lower_limit: 0.0 + * soft_upper_limit: 1.0 + * k_position: 10.0 + * k_velocity: 10.0 + * \endcode + * + * This specification is similar to the specification of the safety_controller tag in the URDF, adapted to the parameter server. + * + * \param[in] joint_name Name of joint whose limits are to be fetched. + * \param[in] node NodeHandle where the joint limits are specified. + * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite + * existing values. + * \return True if a complete soft limits specification is found (ie. if all \p k_position, \p k_velocity, \p soft_lower_limit and + * \p soft_upper_limit exist in \p joint_limits/joint_name namespace), false otherwise. + */ +inline bool getSoftJointLimits( + const std::string & joint_name, const rclcpp::Node::SharedPtr & node, + SoftJointLimits & soft_limits) +{ + const std::string param_base_name = "joint_limits." + joint_name; + try { + if (!node->has_parameter(param_base_name + ".has_soft_limits") && + !node->has_parameter(param_base_name + ".k_velocity") && + !node->has_parameter(param_base_name + ".k_position") && + !node->has_parameter(param_base_name + ".soft_lower_limit") && + !node->has_parameter(param_base_name + ".soft_upper_limit")) + { + RCLCPP_DEBUG_STREAM( + node->get_logger(), "No soft joint limits specification found for joint '" << joint_name << + "' in the parameter server (node: " << std::string( + node->get_name()) + " param name: " + param_base_name << ")."); + return false; + } + } catch (const std::exception & ex) { + RCLCPP_ERROR_STREAM(node->get_logger(), ex.what()); + return false; + } + + // Override soft limits if complete specification is found + bool has_soft_limits; + if (node->get_parameter(param_base_name + ".has_soft_limits", has_soft_limits)) { + if (has_soft_limits && + node->has_parameter(param_base_name + ".k_position") && + node->has_parameter(param_base_name + ".k_velocity") && + node->has_parameter(param_base_name + ".soft_lower_limit") && + node->has_parameter(param_base_name + ".soft_upper_limit")) + { + node->get_parameter(param_base_name + ".k_position", soft_limits.k_position); + node->get_parameter(param_base_name + ".k_velocity", soft_limits.k_velocity); + node->get_parameter(param_base_name + ".soft_lower_limit", soft_limits.min_position); + node->get_parameter(param_base_name + ".soft_upper_limit", soft_limits.max_position); + return true; + } + } + + return false; +} + +} // namespace joint_limits_interface + +#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_ROSPARAM_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 new file mode 100644 index 0000000000..e86a91e910 --- /dev/null +++ b/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp @@ -0,0 +1,103 @@ +// Copyright 2013, PAL Robotics S.L. All rights reserved. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the copyright holders nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/// \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 +{ + +/** + * \brief 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; +} + +/** + * \brief 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 new file mode 100644 index 0000000000..6ee1c8da2b --- /dev/null +++ b/joint_limits_interface/mainpage.dox @@ -0,0 +1,142 @@ +/** +\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.enforceLimits(period) + jnt_limits_interface_.enforceLimits(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 new file mode 100644 index 0000000000..ebf7d3a279 --- /dev/null +++ b/joint_limits_interface/package.xml @@ -0,0 +1,39 @@ + + joint_limits_interface + 0.16.0 + Interface for enforcing joint limits. + + Bence Magyar + Enrique Fernandez + Mathias Lüdtke + + BSD + + https://github.com/ros-controls/ros_control/wiki + https://github.com/ros2-controls/ros_control/issues + https://github.com/ros2-controls/ros_control + + Adolfo Rodriguez Tsouroukdissian + + ament_cmake + + rclcpp + urdf + + hardware_interface + + hardware_interface + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + hardware_interface + launch + launch_ros + launch_testing + launch_testing_ament_cmake + + + ament_cmake + + diff --git a/joint_limits_interface/test/joint_limits_interface_test.cpp b/joint_limits_interface/test/joint_limits_interface_test.cpp new file mode 100644 index 0000000000..c677d9e535 --- /dev/null +++ b/joint_limits_interface/test/joint_limits_interface_test.cpp @@ -0,0 +1,615 @@ +// Copyright 2013, PAL Robotics S.L. All rights reserved. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the copyright holders nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#include + +#include + +#include +#include + +#include + +#include + + +// Floating-point value comparison threshold +const double EPS = 1e-12; + +TEST(SaturateTest, Saturate) +{ + const double min = -1.0; + const double max = 2.0; + double val; + + val = -0.5; + EXPECT_NEAR(val, rcppmath::clamp(val, min, max), EPS); + + val = 0.5; + EXPECT_NEAR(val, rcppmath::clamp(val, min, max), EPS); + + val = -1.0; + EXPECT_NEAR(val, rcppmath::clamp(min, min, max), EPS); + + val = -2.0; + EXPECT_NEAR(min, rcppmath::clamp(val, min, max), EPS); + + val = 2.0; + EXPECT_NEAR(val, rcppmath::clamp(val, min, max), EPS); + + val = 3.0; + EXPECT_NEAR(max, rcppmath::clamp(val, min, max), EPS); +} + + +class JointLimitsTest +{ +public: + JointLimitsTest() + : pos(0.0), vel(0.0), eff(0.0), cmd(0.0), + name("joint_name"), + period(0, 100000000), + cmd_handle(name, &cmd), + state_handle(name, &pos, &vel, &eff) + { + limits.has_position_limits = true; + limits.min_position = -1.0; + limits.max_position = 1.0; + + limits.has_velocity_limits = true; + limits.max_velocity = 2.0; + + limits.has_effort_limits = true; + limits.max_effort = 8.0; + + soft_limits.min_position = -0.8; + soft_limits.max_position = 0.8; + soft_limits.k_position = 20.0; + // TODO(anyone): Tune value + soft_limits.k_velocity = 40.0; + } + +protected: + double pos, vel, eff, cmd; + std::string name; + rclcpp::Duration period; + hardware_interface::JointCommandHandle cmd_handle; + hardware_interface::JointStateHandle state_handle; + joint_limits_interface::JointLimits limits; + joint_limits_interface::SoftJointLimits soft_limits; +}; + +class JointLimitsHandleTest : public JointLimitsTest, public ::testing::Test {}; + +TEST_F(JointLimitsHandleTest, HandleConstruction) +{ + { + joint_limits_interface::JointLimits limits_bad; + EXPECT_THROW( + joint_limits_interface::PositionJointSoftLimitsHandle( + state_handle, cmd_handle, limits_bad, + soft_limits), joint_limits_interface::JointLimitsInterfaceException); + + // Print error messages. Requires manual output inspection, but exception message should be + // descriptive + try { + joint_limits_interface::PositionJointSoftLimitsHandle( + state_handle, cmd_handle, limits_bad, soft_limits); + } catch (const joint_limits_interface::JointLimitsInterfaceException & e) { + RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); + } + } + + { + joint_limits_interface::JointLimits limits_bad; + limits_bad.has_effort_limits = true; + EXPECT_THROW( + joint_limits_interface::EffortJointSoftLimitsHandle( + state_handle, cmd_handle, limits_bad, + soft_limits), joint_limits_interface::JointLimitsInterfaceException); + + // Print error messages. Requires manual output inspection, + // but exception message should be descriptive + try { + joint_limits_interface::EffortJointSoftLimitsHandle( + state_handle, cmd_handle, limits_bad, soft_limits); + } catch (const joint_limits_interface::JointLimitsInterfaceException & e) { + RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); + } + } + + { + joint_limits_interface::JointLimits limits_bad; + limits_bad.has_velocity_limits = true; + EXPECT_THROW( + joint_limits_interface::EffortJointSoftLimitsHandle( + state_handle, cmd_handle, limits_bad, + soft_limits), joint_limits_interface::JointLimitsInterfaceException); + + // Print error messages. Requires manual output inspection, but exception message should + // be descriptive + try { + joint_limits_interface::EffortJointSoftLimitsHandle( + state_handle, cmd_handle, limits_bad, soft_limits); + } catch (const joint_limits_interface::JointLimitsInterfaceException & e) { + RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); + } + } + + { + joint_limits_interface::JointLimits limits_bad; + EXPECT_THROW( + joint_limits_interface::VelocityJointSaturationHandle( + state_handle, cmd_handle, + limits_bad), joint_limits_interface::JointLimitsInterfaceException); + + // Print error messages. Requires manual output inspection, but exception message should + // be descriptive + try { + joint_limits_interface::VelocityJointSaturationHandle(state_handle, cmd_handle, limits_bad); + } catch (const joint_limits_interface::JointLimitsInterfaceException & e) { + RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); + } + } + + EXPECT_NO_THROW( + joint_limits_interface::PositionJointSoftLimitsHandle( + state_handle, cmd_handle, limits, soft_limits)); + EXPECT_NO_THROW( + joint_limits_interface::EffortJointSoftLimitsHandle( + state_handle, cmd_handle, limits, soft_limits)); + EXPECT_NO_THROW( + joint_limits_interface::VelocityJointSaturationHandle( + state_handle, cmd_handle, limits)); +} + +class PositionJointSoftLimitsHandleTest : public JointLimitsTest, public ::testing::Test {}; + +TEST_F(PositionJointSoftLimitsHandleTest, EnforceVelocityBounds) +{ + // Test setup + const double max_increment = period.seconds() * limits.max_velocity; + pos = 0.0; + + double cmd; + + // Move slower than maximum velocity + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + state_handle, cmd_handle, limits, soft_limits); + cmd = max_increment / 2.0; + cmd_handle.set_cmd(cmd); + limits_handle.enforceLimits(period); + EXPECT_NEAR(cmd, cmd_handle.get_cmd(), EPS); + } + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + state_handle, cmd_handle, limits, soft_limits); + cmd = -max_increment / 2.0; + cmd_handle.set_cmd(cmd); + limits_handle.enforceLimits(period); + EXPECT_NEAR(cmd, cmd_handle.get_cmd(), EPS); + } + + // Move at maximum velocity + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + state_handle, cmd_handle, limits, soft_limits); + cmd = max_increment; + cmd_handle.set_cmd(cmd); + limits_handle.enforceLimits(period); + EXPECT_NEAR(cmd, cmd_handle.get_cmd(), EPS); + } + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + state_handle, cmd_handle, limits, soft_limits); + cmd = -max_increment; + cmd_handle.set_cmd(cmd); + limits_handle.enforceLimits(period); + EXPECT_NEAR(cmd, cmd_handle.get_cmd(), EPS); + } + + // Try to move faster than the maximum velocity, enforce velocity limits + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + state_handle, cmd_handle, limits, soft_limits); + cmd = 2.0 * max_increment; + cmd_handle.set_cmd(cmd); + limits_handle.enforceLimits(period); + EXPECT_NEAR(max_increment, cmd_handle.get_cmd(), EPS); + } + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + state_handle, cmd_handle, limits, soft_limits); + cmd = -2.0 * max_increment; + cmd_handle.set_cmd(cmd); + limits_handle.enforceLimits(period); + EXPECT_NEAR(-max_increment, cmd_handle.get_cmd(), EPS); + } +} + +// This is a black box test and does not verify against random precomputed values, but rather that +// the expected qualitative behavior is honored +TEST_F(PositionJointSoftLimitsHandleTest, EnforcePositionBounds) +{ + // Test setup + const double workspace_center = (limits.min_position + limits.max_position) / 2.0; + + // Current position == upper soft limit + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + state_handle, cmd_handle, limits, soft_limits); + + // Can't get any closer to hard limit (zero max velocity) + pos = soft_limits.max_position; + // Try to get closer to the hard limit + cmd_handle.set_cmd(limits.max_position); + limits_handle.enforceLimits(period); + EXPECT_NEAR(state_handle.get_position(), cmd_handle.get_cmd(), EPS); + + // OK to move away from hard limit + // Try to go to workspace center + cmd_handle.set_cmd(workspace_center); + limits_handle.enforceLimits(period); + EXPECT_GT(state_handle.get_position(), cmd_handle.get_cmd()); + } + + // Current position == lower soft limit + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + state_handle, cmd_handle, limits, soft_limits); + + // Can't get any closer to hard limit (zero min velocity) + pos = soft_limits.min_position; + // Try to get closer to the hard limit + cmd_handle.set_cmd(limits.min_position); + limits_handle.enforceLimits(period); + EXPECT_NEAR(state_handle.get_position(), cmd_handle.get_cmd(), EPS); + + // OK to move away from hard limit + // Try to go to workspace center + cmd_handle.set_cmd(workspace_center); + limits_handle.enforceLimits(period); + EXPECT_LT(state_handle.get_position(), cmd_handle.get_cmd()); + } + + // Current position > upper soft limit + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + state_handle, cmd_handle, limits, soft_limits); + + // Can't get any closer to hard limit (negative max velocity) + // Halfway between soft and hard limit + pos = (soft_limits.max_position + limits.max_position) / 2.0; + // Try to get closer to the hard limit + cmd_handle.set_cmd(limits.max_position); + limits_handle.enforceLimits(period); + EXPECT_GT(state_handle.get_position(), cmd_handle.get_cmd()); + + // OK to move away from hard limit + // Try to go to workspace center + cmd_handle.set_cmd(workspace_center); + limits_handle.enforceLimits(period); + EXPECT_GT(state_handle.get_position(), cmd_handle.get_cmd()); + } + + // Current position < lower soft limit + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + state_handle, cmd_handle, limits, soft_limits); + + // Can't get any closer to hard limit (positive min velocity) + // Halfway between soft and hard limit + pos = (soft_limits.min_position + limits.min_position) / 2.0; + // Try to get closer to the hard limit + cmd_handle.set_cmd(limits.min_position); + limits_handle.enforceLimits(period); + EXPECT_LT(state_handle.get_position(), cmd_handle.get_cmd()); + + // OK to move away from hard limit + // Try to go to workspace center + cmd_handle.set_cmd(workspace_center); + limits_handle.enforceLimits(period); + EXPECT_LT(state_handle.get_position(), cmd_handle.get_cmd()); + } +} + +TEST_F(PositionJointSoftLimitsHandleTest, PathologicalSoftBounds) +{ + // Safety limits are past the hard limits + soft_limits.min_position = limits.min_position * + (1.0 - 0.5 * limits.min_position / std::abs(limits.min_position)); + soft_limits.max_position = limits.max_position * + (1.0 + 0.5 * limits.max_position / std::abs(limits.max_position)); + + // Current position == higher hard limit + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + state_handle, cmd_handle, limits, soft_limits); + + // Hit hard limit + // On hard limit + pos = limits.max_position; + // Way beyond hard limit + cmd_handle.set_cmd(2.0 * limits.max_position); + limits_handle.enforceLimits(period); + EXPECT_NEAR(limits.max_position, cmd_handle.get_cmd(), EPS); + } + + // Current position == lower hard limit + { + joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( + state_handle, cmd_handle, limits, soft_limits); + + // Hit hard limit + // On hard limit + pos = limits.min_position; + // Way beyond hard limit + cmd_handle.set_cmd(2.0 * limits.min_position); + limits_handle.enforceLimits(period); + EXPECT_NEAR(limits.min_position, cmd_handle.get_cmd(), EPS); + } +} + +class VelocityJointSaturationHandleTest : public JointLimitsTest, public ::testing::Test {}; + +TEST_F(VelocityJointSaturationHandleTest, EnforceVelocityBounds) +{ + // Test setup + joint_limits_interface::VelocityJointSaturationHandle limits_handle( + state_handle, cmd_handle, limits); + + pos = 0.0; + double cmd; + + // Velocity within bounds + cmd = limits.max_velocity / 2.0; + cmd_handle.set_cmd(cmd); + limits_handle.enforceLimits(period); + EXPECT_NEAR(cmd, cmd_handle.get_cmd(), EPS); + + cmd = -limits.max_velocity / 2.0; + cmd_handle.set_cmd(cmd); + limits_handle.enforceLimits(period); + EXPECT_NEAR(cmd, cmd_handle.get_cmd(), EPS); + + // Velocity at bounds + cmd = limits.max_velocity; + cmd_handle.set_cmd(cmd); + limits_handle.enforceLimits(period); + EXPECT_NEAR(cmd, cmd_handle.get_cmd(), EPS); + + cmd = -limits.max_velocity; + cmd_handle.set_cmd(cmd); + limits_handle.enforceLimits(period); + EXPECT_NEAR(cmd, cmd_handle.get_cmd(), EPS); + + // Velocity beyond bounds + cmd = 2.0 * limits.max_velocity; + cmd_handle.set_cmd(cmd); + limits_handle.enforceLimits(period); + EXPECT_NEAR(limits.max_velocity, cmd_handle.get_cmd(), EPS); + + cmd = -2.0 * limits.max_velocity; + cmd_handle.set_cmd(cmd); + limits_handle.enforceLimits(period); + EXPECT_NEAR(-limits.max_velocity, cmd_handle.get_cmd(), EPS); +} + +TEST_F(VelocityJointSaturationHandleTest, EnforceAccelerationBounds) +{ + // Test setup + limits.has_acceleration_limits = true; + limits.max_acceleration = limits.max_velocity / period.seconds(); + joint_limits_interface::VelocityJointSaturationHandle limits_handle( + state_handle, cmd_handle, limits); + + pos = 0.0; + double cmd; + // An arbitrarily long time, sufficient to suppress acceleration limits + const rclcpp::Duration long_enough(1000, 0); + + // Positive velocity + // register last command + cmd_handle.set_cmd(limits.max_velocity / 2.0); + // make sure the prev_cmd is registered + // without triggering the acceleration limits + limits_handle.enforceLimits(long_enough); + + // Try to go beyond +max velocity + cmd = limits.max_velocity * 2.0; + cmd_handle.set_cmd(cmd); + limits_handle.enforceLimits(period); + // Max velocity bounded by velocity limit + EXPECT_NEAR(limits.max_velocity, cmd_handle.get_cmd(), EPS); + + // register last command + cmd_handle.set_cmd(limits.max_velocity / 2.0); + // make sure the prev_cmd is registered + // without triggering the acceleration limits + limits_handle.enforceLimits(long_enough); + + // Try to go beyond -max velocity + cmd = -limits.max_velocity * 2.0; + cmd_handle.set_cmd(cmd); + limits_handle.enforceLimits(period); + // Max velocity bounded by acceleration limit + EXPECT_NEAR(-limits.max_velocity / 2.0, cmd_handle.get_cmd(), EPS); + + // Negative velocity + // register last command + cmd_handle.set_cmd(-limits.max_velocity / 2.0); + // make sure the prev_cmd is registered + // without triggering the acceleration limits + limits_handle.enforceLimits(long_enough); + + // Try to go beyond +max velocity + cmd = limits.max_velocity * 2.0; + cmd_handle.set_cmd(cmd); + limits_handle.enforceLimits(period); + // Max velocity bounded by acceleration limit + EXPECT_NEAR(limits.max_velocity / 2.0, cmd_handle.get_cmd(), EPS); + + // register last command + cmd_handle.set_cmd(-limits.max_velocity / 2.0); + // make sure the prev_cmd is registered + // without triggering the acceleration limits + limits_handle.enforceLimits(long_enough); + + // Try to go beyond -max velocity + cmd = -limits.max_velocity * 2.0; + cmd_handle.set_cmd(cmd); + limits_handle.enforceLimits(period); + // Max velocity bounded by velocity limit + EXPECT_NEAR(-limits.max_velocity, cmd_handle.get_cmd(), EPS); +} + +class JointLimitsInterfaceTest : public JointLimitsTest, public ::testing::Test +{ +public: + JointLimitsInterfaceTest() + : JointLimitsTest(), + pos2(0.0), vel2(0.0), eff2(0.0), cmd2(0.0), + name2("joint2_name"), + cmd_handle2(name2, &cmd2), + state_handle2(name2, &pos2, &vel2, &eff2) + {} + +protected: + double pos2, vel2, eff2, cmd2; + std::string name2; + hardware_interface::JointCommandHandle cmd_handle2; + hardware_interface::JointStateHandle state_handle2; +}; + +// TEST_F(JointLimitsInterfaceTest, InterfaceRegistration) +// { +// // Populate interface +// joint_limits_interface::PositionJointSoftLimitsHandle limits_handle1( +// state_handle, cmd_handle, limits, soft_limits); +// joint_limits_interface::PositionJointSoftLimitsHandle limits_handle2( +// state_handle2, cmd_handle2, limits, soft_limits); +// +// joint_limits_interface::PositionJointSoftLimitsInterface iface; +// iface.registerHandle(limits_handle1); +// iface.registerHandle(limits_handle2); +// +// // Get handles +// EXPECT_NO_THROW(iface.getHandle(name)); +// EXPECT_NO_THROW(iface.getHandle(name2)); +// +// PositionJointSoftLimitsHandle h1_tmp = iface.getHandle(name); +// EXPECT_EQ(name, h1_tmp.getName()); +// +// PositionJointSoftLimitsHandle h2_tmp = iface.getHandle(name2); +// EXPECT_EQ(name2, h2_tmp.getName()); +// +// // Print error message +// // Requires manual output inspection, but exception message should contain the interface name +// // (not its base class) +// try { +// iface.getHandle("unknown_name"); +// } catch (const JointLimitsInterfaceException & e) { +// ROS_ERROR_STREAM(e.what()); +// } +// +// // Enforce limits of all managed joints +// // Halfway between soft and hard limit +// pos = pos2 = (soft_limits.max_position + limits.max_position) / 2.0; +// // Try to get closer to the hard limit +// cmd_handle.set_cmd(limits.max_position); +// cmd_handle2.set_cmd(limits.max_position); +// iface.enforceLimits(period); +// EXPECT_GT(state_handle.get_position(), cmd_handle.get_cmd()); +// EXPECT_GT(cmd_handle2.getPosition(), cmd_handle2.get_cmd()); +// } +// +// TEST_F(JointLimitsHandleTest, ResetSaturationInterface) +// { +// // Populate interface +// PositionJointSaturationHandle limits_handle1(cmd_handle, limits); +// +// PositionJointSaturationInterface iface; +// iface.registerHandle(limits_handle1); +// +// iface.enforceLimits(period); // initialize limit handles +// +// const double max_increment = period.seconds() * limits.max_velocity; +// +// cmd_handle.set_cmd(limits.max_position); +// iface.enforceLimits(period); +// +// EXPECT_NEAR(cmd_handle.get_cmd(), max_increment, EPS); +// +// iface.reset(); +// pos = limits.max_position; +// cmd_handle.set_cmd(limits.max_position); +// iface.enforceLimits(period); +// +// EXPECT_NEAR(cmd_handle.get_cmd(), limits.max_position, EPS); +// +// } +// +// +// TEST_F(JointLimitsHandleTest, ResetSoftLimitsInterface) +// { +// // Populate interface +// PositionJointSoftLimitsHandle limits_handle1(cmd_handle, limits, soft_limits); +// +// PositionJointSoftLimitsInterface iface; +// iface.registerHandle(limits_handle1); +// +// iface.enforceLimits(period); // initialize limit handles +// +// const double max_increment = period.seconds() * limits.max_velocity; +// +// cmd_handle.set_cmd(limits.max_position); +// iface.enforceLimits(period); +// +// EXPECT_NEAR(cmd_handle.get_cmd(), max_increment, EPS); +// +// iface.reset(); +// pos = limits.max_position; +// cmd_handle.set_cmd(soft_limits.max_position); +// iface.enforceLimits(period); +// +// EXPECT_NEAR(cmd_handle.get_cmd(), soft_limits.max_position, EPS); +// +// } + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/joint_limits_interface/test/joint_limits_rosparam.launch.py b/joint_limits_interface/test/joint_limits_rosparam.launch.py new file mode 100644 index 0000000000..68670574a3 --- /dev/null +++ b/joint_limits_interface/test/joint_limits_rosparam.launch.py @@ -0,0 +1,59 @@ +# Copyright 2020 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +import unittest + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription + +from launch_ros.actions import Node + +import launch_testing +import launch_testing.actions + + +def generate_test_description(): + joint_limits_interface_path = get_package_share_directory('joint_limits_interface') + + node_under_test = Node( + package='joint_limits_interface', + executable='joint_limits_rosparam_test', + output='screen', + parameters=[os.path.join(joint_limits_interface_path, + 'test', + 'joint_limits_rosparam.yaml')], + ) + return LaunchDescription([ + node_under_test, + launch_testing.util.KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ]), locals() + + +class TestJointLimitInterface(unittest.TestCase): + + def test_termination(self, node_under_test, proc_info): + proc_info.assertWaitForShutdown(process=node_under_test, timeout=(10)) + + +@launch_testing.post_shutdown_test() +class TestJointLimitInterfaceTestAfterShutdown(unittest.TestCase): + + def test_exit_code(self, proc_info): + # Check that all processes in the launch (in this case, there's just one) exit + # with code 0 + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/joint_limits_interface/test/joint_limits_rosparam.yaml b/joint_limits_interface/test/joint_limits_rosparam.yaml new file mode 100644 index 0000000000..0ebb43af05 --- /dev/null +++ b/joint_limits_interface/test/joint_limits_rosparam.yaml @@ -0,0 +1,66 @@ +JointLimitsRosparamTestNode: + ros__parameters: + joint_limits: + foo_joint: + has_position_limits: true + min_position: 0.0 + max_position: 1.0 + has_velocity_limits: true + max_velocity: 2.0 + has_acceleration_limits: true + max_acceleration: 5.0 + has_jerk_limits: true + max_jerk: 100.0 + has_effort_limits: true + max_effort: 20.0 + angle_wraparound: true # should be ignored, has position limits + has_soft_limits: true + k_position: 10.0 + k_velocity: 20.0 + soft_lower_limit: 0.1 + soft_upper_limit: 0.9 + + yinfoo_joint: + has_position_limits: true + has_velocity_limits: true + has_acceleration_limits: true + has_jerk_limits: true + has_effort_limits: true + + yangfoo_joint: + min_position: 0.0 + max_position: 1.0 + max_velocity: 2.0 + max_acceleration: 5.0 + max_jerk: 100.0 + max_effort: 20.0 + + antifoo_joint: + has_position_limits: false + has_velocity_limits: false + has_acceleration_limits: false + has_jerk_limits: false + has_effort_limits: false + angle_wraparound: true # should be accepted, has no position limits + + bar_joint: + has_velocity_limits: true + max_velocity: 2.0 + + baz_joint: + has_position_limits: true + # Missing min_position + max_position: 1.0 + + foobar_joint: + has_soft_limits: false + k_velocity: 20.0 + k_position: 10.0 + soft_lower_limit: 0.1 + soft_upper_limit: 0.9 + + barbaz_joint: + has_soft_limits: false + k_position: 10.0 + soft_lower_limit: 0.1 + soft_upper_limit: 0.9 diff --git a/joint_limits_interface/test/joint_limits_rosparam_test.cpp b/joint_limits_interface/test/joint_limits_rosparam_test.cpp new file mode 100644 index 0000000000..f6855d32d7 --- /dev/null +++ b/joint_limits_interface/test/joint_limits_rosparam_test.cpp @@ -0,0 +1,218 @@ +// Copyright 2013, PAL Robotics S.L. All rights reserved. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the copyright holders nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#include + +#include + +#include + +#include + +class JointLimitsRosParamTest : public ::testing::Test +{ +protected: + void SetUp() + { + rclcpp::NodeOptions node_options; + node_options.allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true); + + node_ = rclcpp::Node::make_shared("JointLimitsRosparamTestNode", node_options); + } + + rclcpp::Node::SharedPtr node_; +}; + +TEST_F(JointLimitsRosParamTest, GetJointLimits) +{ + // Invalid specification + { + joint_limits_interface::JointLimits limits; + EXPECT_FALSE(getJointLimits("bad_joint", node_, limits)); + EXPECT_FALSE(getJointLimits("unknown_joint", node_, limits)); + + EXPECT_FALSE(limits.has_position_limits); + EXPECT_FALSE(limits.has_velocity_limits); + EXPECT_FALSE(limits.has_acceleration_limits); + EXPECT_FALSE(limits.has_jerk_limits); + EXPECT_FALSE(limits.has_effort_limits); + } + + // Get full specification from parameter server + { + joint_limits_interface::JointLimits limits; + EXPECT_TRUE(getJointLimits("foo_joint", node_, limits)); + + EXPECT_TRUE(limits.has_position_limits); + EXPECT_EQ(0.0, limits.min_position); + EXPECT_EQ(1.0, limits.max_position); + + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_EQ(2.0, limits.max_velocity); + + EXPECT_TRUE(limits.has_acceleration_limits); + EXPECT_EQ(5.0, limits.max_acceleration); + + EXPECT_TRUE(limits.has_jerk_limits); + EXPECT_EQ(100.0, limits.max_jerk); + + EXPECT_TRUE(limits.has_effort_limits); + EXPECT_EQ(20.0, limits.max_effort); + + EXPECT_FALSE(limits.angle_wraparound); + } + + // Specifying flags but not values should set nothing + { + joint_limits_interface::JointLimits limits; + EXPECT_TRUE(getJointLimits("yinfoo_joint", node_, limits)); + + EXPECT_FALSE(limits.has_position_limits); + EXPECT_FALSE(limits.has_velocity_limits); + EXPECT_FALSE(limits.has_acceleration_limits); + EXPECT_FALSE(limits.has_jerk_limits); + EXPECT_FALSE(limits.has_effort_limits); + } + + // Specifying values but not flags should set nothing + { + joint_limits_interface::JointLimits limits; + EXPECT_TRUE(getJointLimits("yangfoo_joint", node_, limits)); + + EXPECT_FALSE(limits.has_position_limits); + EXPECT_FALSE(limits.has_velocity_limits); + EXPECT_FALSE(limits.has_acceleration_limits); + EXPECT_FALSE(limits.has_jerk_limits); + EXPECT_FALSE(limits.has_effort_limits); + } + + // Disable already set values + { + joint_limits_interface::JointLimits limits; + EXPECT_TRUE(getJointLimits("foo_joint", node_, limits)); + EXPECT_TRUE(limits.has_position_limits); + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_TRUE(limits.has_acceleration_limits); + EXPECT_TRUE(limits.has_jerk_limits); + EXPECT_TRUE(limits.has_effort_limits); + + EXPECT_TRUE(getJointLimits("antifoo_joint", node_, limits)); + EXPECT_FALSE(limits.has_position_limits); + EXPECT_FALSE(limits.has_velocity_limits); + EXPECT_FALSE(limits.has_acceleration_limits); + EXPECT_FALSE(limits.has_jerk_limits); + EXPECT_FALSE(limits.has_effort_limits); + EXPECT_TRUE(limits.angle_wraparound); + } + + // Incomplete position limits specification does not get loaded + { + joint_limits_interface::JointLimits limits; + EXPECT_TRUE(getJointLimits("baz_joint", node_, limits)); + + EXPECT_FALSE(limits.has_position_limits); + } + + // Override only one field, leave all others unchanged + { + joint_limits_interface::JointLimits limits, limits_ref; + EXPECT_TRUE(getJointLimits("bar_joint", node_, limits)); + + EXPECT_EQ(limits_ref.has_position_limits, limits.has_position_limits); + EXPECT_EQ(limits_ref.min_position, limits.min_position); + EXPECT_EQ(limits_ref.max_position, limits.max_position); + + // Max velocity is overridden + EXPECT_NE(limits_ref.has_velocity_limits, limits.has_velocity_limits); + EXPECT_NE(limits_ref.max_velocity, limits.max_velocity); + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_EQ(2.0, limits.max_velocity); + + EXPECT_EQ(limits_ref.has_acceleration_limits, limits.has_acceleration_limits); + EXPECT_EQ(limits_ref.max_acceleration, limits.max_acceleration); + + EXPECT_EQ(limits_ref.has_jerk_limits, limits.has_jerk_limits); + EXPECT_EQ(limits_ref.has_jerk_limits, limits.max_jerk); + + EXPECT_EQ(limits_ref.has_effort_limits, limits.has_effort_limits); + EXPECT_EQ(limits_ref.max_effort, limits.max_effort); + } +} + +TEST_F(JointLimitsRosParamTest, GetSoftJointLimits) +{ + // Invalid specification + { + joint_limits_interface::SoftJointLimits soft_limits; + EXPECT_FALSE(getSoftJointLimits("bad_joint", node_, soft_limits)); + EXPECT_FALSE(getSoftJointLimits("unknown_joint", node_, soft_limits)); + } + + // Get full specification from parameter server + { + joint_limits_interface::SoftJointLimits soft_limits; + EXPECT_TRUE(getSoftJointLimits("foo_joint", node_, soft_limits)); + + EXPECT_EQ(10.0, soft_limits.k_position); + EXPECT_EQ(20.0, soft_limits.k_velocity); + EXPECT_EQ(0.1, soft_limits.min_position); + EXPECT_EQ(0.9, soft_limits.max_position); + } + + // Skip parsing soft limits if has_soft_limits is false + { + joint_limits_interface::SoftJointLimits soft_limits, soft_limits_ref; + EXPECT_FALSE(getSoftJointLimits("foobar_joint", node_, soft_limits)); + } + + // Incomplete soft limits specification does not get loaded + { + joint_limits_interface::SoftJointLimits soft_limits, soft_limits_ref; + EXPECT_FALSE(getSoftJointLimits("barbaz_joint", node_, soft_limits)); + EXPECT_EQ(soft_limits.k_position, soft_limits_ref.k_position); + EXPECT_EQ(soft_limits.k_velocity, soft_limits_ref.k_velocity); + EXPECT_EQ(soft_limits.min_position, soft_limits_ref.min_position); + EXPECT_EQ(soft_limits.max_position, soft_limits_ref.max_position); + } +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/joint_limits_interface/test/joint_limits_urdf_test.cpp b/joint_limits_interface/test/joint_limits_urdf_test.cpp new file mode 100644 index 0000000000..d1c0b6a979 --- /dev/null +++ b/joint_limits_interface/test/joint_limits_urdf_test.cpp @@ -0,0 +1,194 @@ +// Copyright 2013, PAL Robotics S.L. All rights reserved. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the copyright holders nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/// \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(); +}