Add header to import limits from standard URDF definition (ros-contro…

…ls#1298)

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();
+}