URDF for Humanoid Robots
URDF (Unified Robot Description Format) is the standard way to describe robot models in ROS. This chapter focuses on creating URDF files for humanoid robots, covering everything from basic structure to complex kinematic chains and visual properties.
Learning Objectives
By the end of this chapter, you will be able to:
- Create URDF files describing humanoid robot structure
- Define joints, links, and kinematic chains for bipedal robots
- Add visual and collision properties to robot models
- Use Xacro for parameterized and reusable URDF descriptions
- Integrate URDF with ROS 2 for visualization and simulation
- Understand the relationship between URDF and robot kinematics
Understanding URDF
URDF (Unified Robot Description Format) is an XML-based format that describes robot models in ROS. It defines the physical and visual properties of a robot, including:
- Links: Rigid bodies of the robot
- Joints: Connections between links
- Visual properties: How the robot appears in visualization
- Collision properties: How the robot interacts in simulation
- Inertial properties: Mass, center of mass, and inertia for physics simulation
Basic URDF Structure
<?xml version="1.0"?>
<robot name="my_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Links define rigid bodies -->
<link name="base_link">
<visual>
<geometry>
<box size="0.5 0.5 0.2"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.5 0.5 0.2"/>
</geometry>
</collision>
<inertial>
<mass value="1.0"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
</link>
<!-- Joints connect links -->
<joint name="joint_name" type="revolute">
<parent link="base_link"/>
<child link="child_link"/>
<origin xyz="0 0 0.1" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="-1.57" upper="1.57" effort="100" velocity="1"/>
</joint>
</robot>
Link Elements
Links represent rigid bodies in the robot. Each link can have multiple properties:
Visual Properties
<link name="link_name">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<!-- Different geometry types -->
<box size="0.1 0.1 0.1"/>
<!-- OR -->
<cylinder radius="0.05" length="0.1"/>
<!-- OR -->
<sphere radius="0.05"/>
<!-- OR -->
<mesh filename="package://my_robot_description/meshes/link_name.dae"/>
</geometry>
<material name="color_name">
<color rgba="0.8 0.2 0.2 1.0"/>
</material>
</visual>
</link>
Collision Properties
<link name="link_name">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<!-- Often simpler than visual geometry for performance -->
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
</link>
Inertial Properties
<link name="link_name">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.5"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
</inertial>
</link>
Joint Elements
Joints define how links connect and move relative to each other:
Joint Types
- revolute: Rotational joint with limits
- continuous: Rotational joint without limits
- prismatic: Linear sliding joint with limits
- fixed: No movement (welded connection)
- floating: 6-DOF movement
- planar: Movement in a plane
Joint Definition Example
<joint name="hip_joint" type="revolute">
<parent link="torso"/>
<child link="thigh"/>
<origin xyz="0 0 -0.1" rpy="0 0 0"/>
<axis xyz="0 1 0"/> <!-- Rotation around Y axis -->
<limit lower="-1.57" upper="1.57" effort="100" velocity="2"/>
<dynamics damping="0.1" friction="0.0"/>
</joint>
Humanoid Robot URDF Structure
A typical humanoid robot has a hierarchical structure:
base_link (world)
└── pelvis
├── torso
│ ├── head
│ ├── left_arm
│ │ ├── left_forearm
│ │ └── left_hand
│ └── right_arm
│ ├── right_forearm
│ └── right_hand
├── left_leg
│ ├── left_lower_leg
│ └── left_foot
└── right_leg
├── right_lower_leg
└── right_foot
Complete Humanoid URDF Example
<?xml version="1.0"?>
<robot name="humanoid_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Base link -->
<link name="base_link"/>
<!-- Pelvis -->
<link name="pelvis">
<visual>
<geometry>
<box size="0.2 0.3 0.1"/>
</geometry>
<material name="gray">
<color rgba="0.5 0.5 0.5 1.0"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.2 0.3 0.1"/>
</geometry>
</collision>
<inertial>
<mass value="5.0"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
</link>
<joint name="base_to_pelvis" type="fixed">
<parent link="base_link"/>
<child link="pelvis"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>
<!-- Torso -->
<link name="torso">
<visual>
<geometry>
<box size="0.2 0.1 0.5"/>
</geometry>
<material name="blue">
<color rgba="0.2 0.2 0.8 1.0"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.2 0.1 0.5"/>
</geometry>
</collision>
<inertial>
<mass value="8.0"/>
<inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
</inertial>
</link>
<joint name="pelvis_to_torso" type="revolute">
<parent link="pelvis"/>
<child link="torso"/>
<origin xyz="0 0 0.05" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="-0.5" upper="0.5" effort="200" velocity="3"/>
</joint>
<!-- Head -->
<link name="head">
<visual>
<geometry>
<sphere radius="0.1"/>
</geometry>
<material name="white">
<color rgba="0.9 0.9 0.9 1.0"/>
</material>
</visual>
<collision>
<geometry>
<sphere radius="0.1"/>
</geometry>
</collision>
<inertial>
<mass value="2.0"/>
<inertia ixx="0.02" ixy="0" ixz="0" iyy="0.02" iyz="0" izz="0.02"/>
</inertial>
</link>
<joint name="torso_to_head" type="revolute">
<parent link="torso"/>
<child link="head"/>
<origin xyz="0 0 0.3" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="-0.5" upper="0.5" effort="50" velocity="2"/>
</joint>
<!-- Left Arm -->
<link name="left_shoulder">
<visual>
<geometry>
<cylinder radius="0.05" length="0.1"/>
</geometry>
<material name="red">
<color rgba="0.8 0.2 0.2 1.0"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.05" length="0.1"/>
</geometry>
</collision>
<inertial>
<mass value="1.0"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
</inertial>
</link>
<joint name="torso_to_left_shoulder" type="revolute">
<parent link="torso"/>
<child link="left_shoulder"/>
<origin xyz="-0.1 0 0.2" rpy="0 0 -1.57"/>
<axis xyz="1 0 0"/>
<limit lower="-1.57" upper="1.57" effort="100" velocity="2"/>
</joint>
<!-- Additional links and joints would continue for full humanoid model -->
</robot>
Using Xacro for Parameterized URDF
Xacro is a macro language that extends URDF with variables, math, and includes:
Basic Xacro Example
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="param_robot">
<!-- Define properties -->
<xacro:property name="M_PI" value="3.1415926535897931" />
<xacro:property name="base_width" value="0.5" />
<xacro:property name="base_length" value="0.3" />
<xacro:property name="base_height" value="0.2" />
<xacro:property name="base_mass" value="10.0" />
<!-- Macro for creating a box link -->
<xacro:macro name="box_link" params="name xyz size mass">
<link name="${name}">
<visual>
<origin xyz="${xyz}"/>
<geometry>
<box size="${size}"/>
</geometry>
<material name="light_gray">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<origin xyz="${xyz}"/>
<geometry>
<box size="${size}"/>
</geometry>
</collision>
<inertial>
<mass value="${mass}"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
</link>
</xacro:macro>
<!-- Use the macro -->
<xacro:box_link name="base_link"
xyz="0 0 0"
size="${base_length} ${base_width} ${base_height}"
mass="${base_mass}"/>
</robot>
Xacro for Humanoid Robot
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="humanoid_robot">
<!-- Robot dimensions -->
<xacro:property name="torso_height" value="0.5" />
<xacro:property name="torso_width" value="0.2" />
<xacro:property name="torso_depth" value="0.1" />
<xacro:property name="arm_length" value="0.4" />
<xacro:property name="arm_radius" value="0.05" />
<xacro:property name="leg_length" value="0.6" />
<xacro:property name="leg_radius" value="0.06" />
<!-- Include other xacro files -->
<xacro:include filename="$(find my_robot_description)/urdf/materials.xacro" />
<xacro:include filename="$(find my_robot_description)/urdf/transmission.xacro" />
<!-- Macro for humanoid limb -->
<xacro:macro name="humanoid_limb" params="side type">
<link name="${side}_${type}_root">
<visual>
<geometry>
<cylinder radius="${arm_radius}" length="0.1"/>
</geometry>
<material name="limb_material"/>
</visual>
<collision>
<geometry>
<cylinder radius="${arm_radius}" length="0.1"/>
</geometry>
</collision>
<inertial>
<mass value="1.0"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
</inertial>
</link>
</xacro:macro>
<!-- Robot base -->
<link name="base_link">
<visual>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<inertial>
<mass value="1.0"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
</inertial>
</link>
<!-- Torso -->
<link name="torso">
<visual>
<geometry>
<box size="${torso_width} ${torso_depth} ${torso_height}"/>
</geometry>
<material name="torso_color"/>
</visual>
<collision>
<geometry>
<box size="${torso_width} ${torso_depth} ${torso_height}"/>
</geometry>
</collision>
<inertial>
<mass value="10.0"/>
<inertia ixx="0.5" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.5"/>
</inertial>
</link>
<joint name="base_to_torso" type="fixed">
<parent link="base_link"/>
<child link="torso"/>
<origin xyz="0 0 0.1" rpy="0 0 0"/>
</joint>
<!-- Create limbs using macros -->
<xacro:humanoid_limb side="left" type="arm"/>
<xacro:humanoid_limb side="right" type="arm"/>
<xacro:humanoid_limb side="left" type="leg"/>
<xacro:humanoid_limb side="right" type="leg"/>
</robot>
URDF Tools and Visualization
Checking URDF Syntax
# Check if URDF is valid XML
check_urdf /path/to/robot.urdf
# Display robot information
urdf_to_graphiz /path/to/robot.urdf
Visualizing URDF
# Launch robot state publisher with URDF
ros2 run robot_state_publisher robot_state_publisher --ros-args -p robot_description:='$(find-pkg-share my_robot_description)/urdf/robot.urdf'
# Visualize in RViz
ros2 run rviz2 rviz2
Robot State Publisher Node
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from geometry_msgs.msg import TransformStamped
from tf2_ros import TransformBroadcaster
import math
class RobotStatePublisher(Node):
def __init__(self):
super().__init__('robot_state_publisher')
# Declare parameter for robot description
self.declare_parameter('robot_description', '')
# Create joint state publisher
self.joint_pub = self.create_publisher(JointState, 'joint_states', 10)
# Create transform broadcaster
self.tf_broadcaster = TransformBroadcaster(self)
# Timer for publishing transforms
self.timer = self.create_timer(0.1, self.publish_joint_states)
# Initialize joint positions
self.joint_positions = {'joint1': 0.0, 'joint2': 0.0}
def publish_joint_states(self):
# Create joint state message
msg = JointState()
msg.name = list(self.joint_positions.keys())
msg.position = list(self.joint_positions.values())
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = 'base_link'
self.joint_pub.publish(msg)
URDF for Simulation
Gazebo-Specific Elements
<gazebo reference="link_name">
<material>Gazebo/Blue</material>
<mu1>0.9</mu1>
<mu2>0.9</mu2>
<kp>1000000.0</kp>
<kd>100.0</kd>
</gazebo>
<!-- Gazebo plugins -->
<gazebo>
<plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
<joint_name>joint_name</joint_name>
</plugin>
</gazebo>
Transmission Elements
<transmission name="tran1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint1">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="motor1">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
Kinematics and URDF
URDF defines the kinematic structure of the robot:
Forward Kinematics
The URDF structure allows ROS to calculate where each link is in space based on joint angles.
Inverse Kinematics
Additional packages like MoveIt! can use the URDF to solve inverse kinematics problems.
Kinematic Chains
<!-- Example of a kinematic chain for an arm -->
<joint name="shoulder_pan_joint" type="revolute">
<parent link="torso"/>
<child link="shoulder"/>
<axis xyz="0 0 1"/>
<limit lower="-2.0" upper="2.0" effort="100" velocity="2"/>
</joint>
<joint name="shoulder_lift_joint" type="revolute">
<parent link="shoulder"/>
<child link="upper_arm"/>
<axis xyz="0 1 0"/>
<limit lower="-2.0" upper="2.0" effort="100" velocity="2"/>
</joint>
<joint name="elbow_joint" type="revolute">
<parent link="upper_arm"/>
<child link="forearm"/>
<axis xyz="0 1 0"/>
<limit lower="-3.0" upper="3.0" effort="100" velocity="2"/>
</joint>
Best Practices for Humanoid URDF
1. Proper Mass Distribution
- Assign realistic masses to each link
- Ensure center of mass is correctly positioned
- Use appropriate inertia values
2. Joint Limits
- Set realistic joint limits based on physical constraints
- Consider safety margins in limits
- Account for gear ratios in velocity/effort limits
3. Visualization vs Collision
- Use detailed meshes for visualization
- Use simplified shapes for collision detection
- Balance detail with performance
4. Naming Conventions
- Use consistent naming (e.g.,
left_leg_hip_joint) - Follow ROS conventions
- Make names descriptive and predictable
5. Modularity
- Use Xacro for reusable components
- Break complex URDFs into smaller files
- Include common elements from separate files
URDF in Humanoid Robotics Context
Applications
- Simulation: Gazebo and other simulators use URDF for physics
- Visualization: RViz displays robot models in real-time
- Planning: MoveIt! uses URDF for motion planning
- Control: Robot controllers use URDF for kinematic calculations
- Calibration: URDF serves as the reference model for calibration
Special Considerations for Humanoids
- Balance: Accurate mass distribution is critical for bipedal stability
- Range of Motion: Joint limits must reflect human-like capabilities
- Manipulation: Hand models need sufficient detail for grasping
- Sensors: Proper placement of IMUs, cameras, and other sensors
Common URDF Issues and Solutions
1. Self-Collision
Issue: Robot links collide with themselves Solution: Adjust collision properties or joint limits
2. Invalid Kinematic Chain
Issue: URDF has loops or disconnected components Solution: Ensure proper parent-child relationships
3. Mass/Inertia Issues
Issue: Robot behaves unrealistically in simulation Solution: Verify mass properties and center of mass
4. Visualization Problems
Issue: Robot appears incorrectly in RViz Solution: Check mesh paths and material definitions
Summary
URDF is fundamental to representing humanoid robots in ROS. It defines the physical structure, kinematic relationships, and visual properties of the robot. Proper URDF design is crucial for simulation, visualization, control, and motion planning. Using Xacro for parameterized descriptions makes URDFs more maintainable and reusable.
Next Steps
In the next module, we'll explore simulation environments starting with Gazebo for creating digital twins of your humanoid robots.
Estimated Reading Time: 28 minutes