Add parsing of safety_limits in URDF

[jcarpent]

Discussed in #2016

^{Originally posted by nicehiro July 20, 2023}
Hi, everyone,

I found that the pinocchio is checking limit of joint to do planning. Is it possible to check soft_lower_limit for planning?

For example, here's an example of tiago dual robot:

  <joint name="arm_right_4_joint" type="revolute">
    <parent link="arm_right_3_link"/>
    <child link="arm_right_4_link"/>
    <origin rpy="-1.5707963267948966 -1.5707963267948966 0.0" xyz="-0.02 -0.027 -0.222"/>
    <axis xyz="0 0 1"/>
    <limit effort="26" lower="-0.39269908169872414" upper="2.356194490192345" velocity="2.35"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.32269908169872413" soft_upper_limit="2.286194490192345"/>
  </joint>

where the value of soft_lower_limit is higher than lower.

See also here for further details on the notion of safety_limits.