diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index a788b6de4f..1ed74c7603 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -8,6 +8,7 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp rclcpp_lifecycle + urdf ) find_package(ament_cmake REQUIRED) @@ -31,6 +32,9 @@ if(BUILD_TESTING) ament_add_gtest_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) target_link_libraries(joint_limits_rosparam_test joint_limits) + ament_add_gtest(joint_limits_urdf_test test/joint_limits_urdf_test.cpp) + target_link_libraries(joint_limits_urdf_test joint_limits) + add_launch_test(test/joint_limits_rosparam.launch.py) install( TARGETS joint_limits_rosparam_test diff --git a/joint_limits/include/joint_limits/joint_limits_urdf.hpp b/joint_limits/include/joint_limits/joint_limits_urdf.hpp new file mode 100644 index 0000000000..b5db39d9fb --- /dev/null +++ b/joint_limits/include/joint_limits/joint_limits_urdf.hpp @@ -0,0 +1,85 @@ +// Copyright 2024 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#ifndef JOINT_LIMITS_URDF_HPP +#define JOINT_LIMITS_URDF_HPP + +#include "joint_limits/joint_limits.hpp" +#include "urdf_model/joint.h" + +namespace joint_limits +{ + +/** + * \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 +#endif // JOINT_LIMITS_URDF_HPP diff --git a/joint_limits/package.xml b/joint_limits/package.xml index e1832fa776..98890eb03a 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -16,6 +16,7 @@ rclcpp rclcpp_lifecycle + urdf launch_testing_ament_cmake ament_cmake_gtest