Parse SoftJointLimits from the URDF

ros-controls · Apr 23, 2024 · 0b7c749 · 0b7c749
1 parent d23775e
commit 0b7c749
Showing 1 changed file with 10 additions and 0 deletions.
diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp
@@ -1022,6 +1022,16 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
         copy_interface_limits(joint.command_interfaces, limits);
       }
       hw_info.limits[joint.name] = limits;
+      joint_limits::SoftJointLimits soft_limits;
+      if (getSoftJointLimits(urdf_joint, soft_limits))
+      {
+        if (limits.has_position_limits)
+        {
+          soft_limits.min_position = std::max(soft_limits.min_position, limits.min_position);
+          soft_limits.max_position = std::min(soft_limits.max_position, limits.max_position);
+        }
+        hw_info.soft_limits[joint.name] = soft_limits;
+      }
     }
   }