Describing the robot (URDF)

The robot in XML

A URDF (Unified Robot Description Format) describes a robot as links (rigid parts) joined by joints. Here is this arm's shoulder:

<joint name="shoulder" type="revolute">
  <parent link="turret_link"/>
  <child link="upper_arm_link"/>
  <origin xyz="0 0 0.08" rpy="0 0 0"/>
  <axis xyz="0 1 0"/>
  <limit lower="-1.9" upper="1.9" effort="20" velocity="1.2"/>
</joint>

origin places the child's frame in the parent's: shifted by xyz (m), turned by rpy (roll, pitch, yaw in rad). The joint then moves along axis: a revolute joint turns about it by its angle, a prismatic one slides along it, and a fixed one doesn't move. A missing origin means zero and a missing axis means 1 0 0. The chain runs base_link → turret_link → upper_arm_link → forearm_link → wrist_link → hand_link → tool0, with two finger links on the hand.

A latched topic

/robot_state_publisher publishes the URDF on /robot_description once, at start-up, with durability TRANSIENT_LOCAL: it keeps the message and hands it to subscribers that join later, but only if they ask for it too. A VOLATILE subscriber (the default) silently gets nothing. Ask with QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL).

Forward kinematics from the URDF

Each joint's transform is its origin followed by its motion: $T_j = \mathrm{Trans}(xyz)\,R_{rpy}\,R(\text{axis}, q)$. To find a link, walk from it to base_link through each joint's parent, then multiply the transforms from the base outwards. rotation and rpy_matrix are given.

Limits

The forward controller passes positions straight to the motors. On a real arm a target past a limit can trip a drive or hit a hard stop, so clamp before you send.

Your task

1. Fix the subscription's QoS so the node gets the description.
2. parse_urdf(xml): return {name: {"type", "parent", "child", "xyz", "rpy", "axis", "lower", "upper"}} with lists of floats, and None limits for joints without a <limit>.
3. fk_from_urdf(joints, positions, link="tool0"): the 4×4 pose of link in base_link, with positions a dict of joint values.
4. clamp_to_limits(joints, positions): the same dict with each value clamped to its joint's limits.

The callback is written: it sends REQUEST clamped. Use Python's xml.etree.ElementTree.