Skip to content

Latest commit

 

History

History
491 lines (378 loc) · 14.3 KB

File metadata and controls

491 lines (378 loc) · 14.3 KB

robot_description Code Guidelines

This document defines the coding conventions and directory structure for the robot_description package. It is intended to guide AI agents (and humans) when adding new robots or modifying existing robot descriptions.


1. Directory Structure

robot_description/
├── urdf/
│   └── {vendor}/
│       └── {robot}/
│           ├── main.urdf           # Base URDF description
│           ├── robot.xacro         # Entry xacro (selects sim / real config)
│           ├── ros2_control.xacro  # ros2_control interface definitions
│           ├── mujoco.xacro        # MuJoCo simulation config
│           └── real.xacro          # Real-hardware config
├── meshes/
│   └── {vendor}/
│       └── {robot}/
│           └── *.STL               # 3D model files
├── mjcf/
│   └── {vendor}/
│       └── {robot}.xml             # Native MuJoCo model
├── rviz/
│   └── {vendor}/
│       └── {robot}/
│           └── main.rviz           # RViz configuration file
├── launch/
│   └── view_robot.launch.py        # Visualization launch file
├── CMakeLists.txt
└── package.xml

1.1 Supported Vendors (vendor)

Vendor Name Directory Robots
Unitree unitree g1
Agibot agibot x2
EngineAI engineai pm01

2. Naming Conventions

2.1 Directory & File Names

  • Vendor directories: lowercase (e.g., unitree/, agibot/)
  • Robot directories: lowercase (e.g., g1/, x2/)
  • Mesh filenames: use link names (e.g., left_hip_pitch_link.STL)

2.2 Joint Names

Format: {side}_{part}_{axis}_joint

Examples:

  • Legs

    • left_hip_pitch_joint / right_hip_pitch_joint
    • left_hip_roll_joint / right_hip_roll_joint
    • left_hip_yaw_joint / right_hip_yaw_joint
    • left_knee_joint / right_knee_joint
    • left_ankle_pitch_joint / right_ankle_pitch_joint
    • left_ankle_roll_joint / right_ankle_roll_joint
  • Waist

    • waist_yaw_joint
    • waist_pitch_joint
    • waist_roll_joint
  • Arms

    • left_shoulder_pitch_joint / right_shoulder_pitch_joint
    • left_shoulder_roll_joint / right_shoulder_roll_joint
    • left_shoulder_yaw_joint / right_shoulder_yaw_joint
    • left_elbow_joint / right_elbow_joint
    • left_wrist_roll_joint / right_wrist_roll_joint
    • left_wrist_pitch_joint / right_wrist_pitch_joint
    • left_wrist_yaw_joint / right_wrist_yaw_joint

2.3 Link Names

Format: {side}_{part}_{axis}_link

  • Base: pelvis (the pelvis is treated as a floating base)
  • Torso: torso_link
  • IMU frames: imu_in_pelvis_link, imu_in_torso_link
  • Foot end-effector frames: LL_FOOT, LR_FOOT (fixed)

2.4 Special Frames

  • base_link — fixed to pelvis, used as the root of the TF tree
  • LL_FOOT / LR_FOOT — foot contact frames (fixed joint)

3. URDF / Xacro File Structure

3.1 main.urdf — Base Model

This is a plain URDF file and should include:

  • All link definitions (including inertial, visual, collision)
  • All joint definitions with limits
  • Mesh file paths using the package:// URI

Example snippet:

<?xml version='1.0' encoding='utf-8'?>
<robot name="{robot_name}">
  <!-- Link definition -->
  <link name="pelvis">
    <inertial>
      <origin xyz="x y z" rpy="0 0 0"/>
      <mass value="m"/>
      <inertia ixx="" ixy="" ixz="" iyy="" iyz="" izz=""/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="package://robot_description/meshes/{vendor}/{robot}/pelvis.STL"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="package://robot_description/meshes/{vendor}/{robot}/pelvis.STL"/>
      </geometry>
    </collision>
  </link>

  <!-- Joint definition -->
  <joint name="left_hip_pitch_joint" type="revolute">
    <origin xyz="x y z" rpy="0 0 0"/>
    <parent link="pelvis"/>
    <child link="left_hip_pitch_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-2.704" upper="2.556" effort="120" velocity="11.936"/>
  </joint>

  <!-- base_link fixed -->
  <link name="base_link"/>
  <joint name="base_link_joint" type="fixed">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="pelvis"/>
  </joint>
</robot>

3.2 robot.xacro — Entry File

The robot.xacro should choose between simulation and real hardware configurations using an argument such as simulation.

Example:

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="robot">
    <xacro:arg name="simulation" default="false"/>
    <xacro:property name="simulation" value="$(arg simulation)"/>

    <!-- Include base URDF -->
    <xacro:include filename="$(find robot_description)/urdf/{vendor}/{robot}/main.urdf"/>

    <!-- MuJoCo simulation config -->
    <xacro:if value="${simulation == 'mujoco'}">
        <xacro:include filename="$(find robot_description)/urdf/{vendor}/{robot}/mujoco.xacro"/>
    </xacro:if>

    <!-- Real hardware config -->
    <xacro:if value="${simulation == False}">
        <xacro:include filename="$(find robot_description)/urdf/{vendor}/{robot}/real.xacro"/>
    </xacro:if>
</robot>

3.3 ros2_control.xacro — ros2_control Interface

This file defines joint command/state interfaces and sensors for ros2_control.

Example macro patterns:

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- Joint interface macro -->
    <xacro:macro name="joint_interface" params="name initial_pos kp:=0 kd:=0">
        <joint name="${name}">
            <command_interface name="stiffness">
                <param name="initial_value">${kp}</param>
            </command_interface>
            <command_interface name="damping">
                <param name="initial_value">${kd}</param>
            </command_interface>
            <state_interface name="position">
                <param name="initial_value">${initial_pos}</param>
            </state_interface>
        </joint>
    </xacro:macro>

    <!-- Hardware interfaces macro -->
    <xacro:macro name="hardware_interface">
        <!-- IMU sensor -->
        <sensor name="base_imu">
            <state_interface name="orientation.x"/>
            <state_interface name="orientation.y"/>
            <state_interface name="orientation.z"/>
            <state_interface name="orientation.w"/>
            <state_interface name="angular_velocity.x"/>
            <state_interface name="angular_velocity.y"/>
            <state_interface name="angular_velocity.z"/>
            <state_interface name="linear_acceleration.x"/>
            <state_interface name="linear_acceleration.y"/>
            <state_interface name="linear_acceleration.z"/>
        </sensor>

        <!-- Leg joints -->
        <xacro:joint_interface name="left_hip_pitch_joint" initial_pos="-0.1"/>
        <xacro:joint_interface name="left_hip_roll_joint" initial_pos="0"/>
        <xacro:joint_interface name="left_hip_yaw_joint" initial_pos="0"/>
        <xacro:joint_interface name="left_knee_joint" initial_pos="0.3"/>
        <xacro:joint_interface name="left_ankle_pitch_joint" initial_pos="-0.2"/>
        <xacro:joint_interface name="left_ankle_roll_joint" initial_pos="0"/>
        <!-- ... right leg, waist, arms ... -->
    </xacro:macro>
</robot>

3.4 mujoco.xacro — MuJoCo Simulation Configuration

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:include filename="$(find robot_description)/urdf/{vendor}/{robot}/ros2_control.xacro"/>

    <ros2_control name="MujocoSystem" type="system">
        <hardware>
            <plugin>mujoco_ros2_control/MujocoSystem</plugin>
        </hardware>
        <xacro:hardware_interface/>
    </ros2_control>
</robot>

3.5 real.xacro — Real Hardware Configuration

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:include filename="$(find robot_description)/urdf/{vendor}/{robot}/ros2_control.xacro"/>

    <!-- Hardware parameters -->
    <xacro:arg name="network_interface" default="eth0"/>
    <xacro:arg name="command_writer_period_us" default="4000"/>
    <xacro:arg name="state_decimation" default="4"/>
    <xacro:arg name="enable_monitor" default="true"/>

    <ros2_control name="{Robot}HardwareInterface" type="system">
        <hardware>
            <plugin>{vendor}/{Robot}HardwareInterface</plugin>
            <param name="network_interface">$(arg network_interface)</param>
            <param name="command_writer_period_us">$(arg command_writer_period_us)</param>
            <param name="state_decimation">$(arg state_decimation)</param>
            <param name="enable_monitor">$(arg enable_monitor)</param>
        </hardware>
        <xacro:hardware_interface/>
    </ros2_control>
</robot>

4. MuJoCo Model (MJCF)

4.1 File Layout

Example MJCF structure:

<mujoco model="{robot}">
    <!-- Compiler settings -->
    <compiler angle="radian" meshdir="../../meshes/{vendor}/{robot}"/>

    <!-- Default class definitions -->
    <default>
        <default class="{robot}">
            <geom contype="0" conaffinity="0"/>
            <joint frictionloss="0.1"/>
            <!-- Joint class defaults -->
        </default>
    </default>

    <!-- Assets -->
    <asset>
        <material name="silver" rgba="0.7 0.7 0.7 1"/>
        <mesh name="pelvis" file="pelvis.STL"/>
        <!-- ... other meshes ... -->
    </asset>

    <!-- World body -->
    <worldbody>
        <body name="pelvis" pos="0 0 0.793" childclass="{robot}">
            <freejoint name="floating_base_joint"/>
            <!-- Kinematic chain -->
        </body>
    </worldbody>

    <!-- Contact pairs -->
    <contact>
        <pair name="left_foot_floor" geom1="left_foot_collision" geom2="floor"/>
        <!-- ... -->
    </contact>

    <!-- Sensors -->
    <sensor>
        <framequat name="base_quat" objtype="site" objname="imu_in_pelvis"/>
        <gyro name="base_gyro" site="imu_in_pelvis"/>
        <accelerometer name="base_accel" site="imu_in_pelvis"/>
    </sensor>
</mujoco>

4.2 Joint Class Conventions

Example joint class:

<default class="hip_pitch">
    <joint axis="0 1 0" range="-2.53 2.88" actuatorfrcrange="-88 88" armature="0.01"/>
</default>

5. ros2_control Interface Specification

5.1 Joint Interfaces

Interface Type Interface Name Purpose
Command stiffness Joint stiffness (Kp)
Command damping Joint damping (Kd)
State position Joint angle (rad)
State velocity Joint velocity (rad/s)
State effort Joint torque (Nm)

5.2 IMU Interfaces

Interface Name Purpose
orientation.x/y/z/w Quaternion orientation
angular_velocity.x/y/z Angular velocity (rad/s)
linear_acceleration.x/y/z Linear acceleration (m/s²)

6. Launch Usage

6.1 Visualize a Robot

# Default (Unitree G1)
ros2 launch robot_description view_robot.launch.py

# Specify organization and robot
ros2 launch robot_description view_robot.launch.py \
    organization:=agibot robot_type:=x2

# Use a specific URDF/xacro file
ros2 launch robot_description view_robot.launch.py \
    organization:=unitree robot_type:=g1 urdf_name:=robot.xacro

6.2 Launch Arguments

Argument Default Options Description
organization unitree unitree, agibot, engineai Vendor
robot_type g1 g1, x2, pm01 Robot model
urdf_name main.urdf Any URDF/xacro filename URDF file to load
rviz_config main.rviz Any RViz file RViz config

7. Steps to Add a New Robot

  1. Create the directory structure:
mkdir -p urdf/{vendor}/{robot}
mkdir -p meshes/{vendor}/{robot}
mkdir -p rviz/{vendor}/{robot}
  1. Add mesh files: put STL files in meshes/{vendor}/{robot}/ named to match link names.

  2. Create main.urdf following the template in section 3.1.

  3. Add ros2_control.xacro defining all joint interfaces (see section 3.3).

  4. Create robot.xacro as the entry xacro (see section 3.2).

  5. Add mujoco.xacro and real.xacro for simulation and real-hardware configurations.

  6. Add an RViz configuration file at rviz/{vendor}/{robot}/main.rviz.

  7. (Optional) Create an MJCF file if you need a native MuJoCo model.

  8. Update the view_robot.launch.py choices to include the new robot.


8. Common Issues

8.1 Mesh Path Format

✅ Correct:

<mesh filename="package://robot_description/meshes/agibot/x2/pelvis.STL"/>

❌ Incorrect:

<mesh filename="meshes/pelvis.STL"/>

8.2 Joint Limit Units

  • URDF joint limits use radians
  • effort is in Nm
  • velocity is in rad/s

8.3 Initial Pose

Set initial joint positions in ros2_control.xacro using initial_value so the robot starts in a stable pose.


9. Relations to Other Packages

robot_description
    ├── referenced by robot_hardware_interface (for URDF path)
    ├── loaded by humanoid_controller (as robot_description parameter)
    └── used by mujoco_ros2_control for simulation

9.1 Loading in Controllers

Example in a launch context:

robot_description = Command([
    FindExecutable(name='xacro'), ' ',
    PathJoinSubstitution([
        FindPackageShare('robot_description'),
        'urdf', organization, robot_type, 'robot.xacro'
    ]),
    ' simulation:=mujoco'
])

9.2 Hardware Interface Plugin Names

Vendor Robot Plugin Name
unitree g1 unitree/G1HardwareInterface
agibot x2 agibot/X2HardwareInterface

10. Quality Checklist

When adding a new robot, ensure:

  • All links have valid inertial parameters
  • Every revolute joint has limit definitions
  • Mesh file URIs use package:// protocol
  • ros2_control exposes interfaces for all controllable joints
  • base_imu sensor interfaces are defined
  • base_link is correctly fixed to pelvis
  • robot.xacro properly switches between sim and real modes
  • RViz config file is provided and valid
  • view_robot.launch.py choices are updated