Merge branch 'iron' into mergify/bp/iron/pr-1413

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

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 = std::abs(urdf_joint->limits->velocity);
+
+ limits.has_acceleration_limits = false;
+
+ limits.has_effort_limits = true;
+ limits.max_effort = std::abs(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

joint_limits/package.xml

@@ -16,6 +16,7 @@
 
  <depend>rclcpp</depend>
  <depend>rclcpp_lifecycle</depend>
+ <depend>urdf</depend>
 
  <test_depend>launch_testing_ament_cmake</test_depend>
  <test_depend>ament_cmake_gtest</test_depend>

joint_limits/test/joint_limits_urdf_test.cpp

@@ -0,0 +1,177 @@
+// 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
+#include "joint_limits/joint_limits_urdf.hpp"
+#include "gtest/gtest.h"
+
+using std::string;
+using namespace joint_limits;
+
+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
+ {
+ JointLimits limits;
+ urdf::JointSharedPtr urdf_joint_bad;
+ EXPECT_FALSE(getJointLimits(urdf_joint_bad, limits));
+ }
+
+ // Unset URDF limits
+ {
+ 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;
+
+ 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;
+
+ 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;
+
+ 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)
+{
+ using namespace joint_limits;
+
+ // Unset URDF joint
+ {
+ SoftJointLimits soft_limits;
+ urdf::JointSharedPtr urdf_joint_bad;
+ EXPECT_FALSE(getSoftJointLimits(urdf_joint_bad, soft_limits));
+ }
+
+ // Unset URDF limits
+ {
+ SoftJointLimits soft_limits;
+ urdf::JointSharedPtr urdf_joint_bad(new urdf::Joint);
+ EXPECT_FALSE(getSoftJointLimits(urdf_joint_bad, soft_limits));
+ }
+
+ // Valid URDF joint
+ {
+ 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();
+}