code-iai · maltehue · Jun 1, 2026 · Jun 1, 2026 · Jun 1, 2026 · Jun 1, 2026
diff --git a/iai_stacy_description/CMakeLists.txt b/iai_stacy_description/CMakeLists.txt
@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.5)
+project(iai_stacy_description)
+
+find_package(ament_cmake REQUIRED)
+find_package(xacro REQUIRED)
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME})
+
+install(DIRECTORY meshes
+  DESTINATION share/${PROJECT_NAME})
+
+install(DIRECTORY urdf
+  DESTINATION share/${PROJECT_NAME})
+
+ament_package()
diff --git a/iai_stacy_description/README.md b/iai_stacy_description/README.md
@@ -0,0 +1,36 @@
+# iai_stacy_description
+
+URDF description and bringup launch files for the Stacy robot at AICOR Institute for Artificial Intelligence, University of Bremen.
+
+## Network Setup
+
+To connect to the robot, plug an ethernet cable directly from the robot controller into your PC.
+Then configure a static IPv4 interface on that network adapter:
+
+| Field   | Value           |
+|---------|-----------------|
+| IP      | 192.168.1.10    |
+| Netmask | 255.255.255.0   |
+| Gateway | 192.168.1.1     |
+
+The robot controller is reachable at `192.168.1.2`.
+
+## Package structure
+
+The hardware bringup uses a two-URDF pattern required by `ur_robot_driver`:
+
+- **`urdf/stacy.urdf.xacro`** — plain description used by `display.launch.py` (no ros2_control tag).
+- **`urdf/stacy_controlled.urdf.xacro`** — hardware description: same geometry plus the `<ros2_control>` hardware interface tag that `ur_control.launch.py` needs.
+- **`launch/rsp.launch.py`** — custom robot state publisher launch that processes `stacy_controlled.urdf.xacro`. Passed to `ur_control.launch.py` via the `description_launchfile` argument, overriding the driver's default `ur_rsp.launch.py` so the full robot (column, camera, tool) is published on `/robot_description` instead of just the bare UR5.
+
+## Launch
+
+**RViz visualization (no hardware required):**
+```bash
+ros2 launch iai_stacy_description display.launch.py
+```
+
+**Real hardware bringup:**
+```bash
+ros2 launch iai_stacy_description hardware.launch.py
+```
diff --git a/iai_stacy_description/launch/display.launch.py b/iai_stacy_description/launch/display.launch.py
@@ -0,0 +1,49 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import Command, LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_share = get_package_share_directory('iai_stacy_description')
+
+    urdf_file = os.path.join(pkg_share, 'urdf', 'stacy.urdf.xacro')
+
+    robot_description = Command(['xacro ', urdf_file])
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'gui',
+            default_value='true',
+            description='Launch joint_state_publisher_gui if true'),
+
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            name='robot_state_publisher',
+            output='screen',
+            parameters=[{'robot_description': robot_description}]),
+
+        Node(
+            package='joint_state_publisher_gui',
+            executable='joint_state_publisher_gui',
+            name='joint_state_publisher_gui',
+            output='screen',
+            condition=IfCondition(LaunchConfiguration('gui'))),
+
+        Node(
+            package='joint_state_publisher',
+            executable='joint_state_publisher',
+            name='joint_state_publisher',
+            output='screen',
+            condition=UnlessCondition(LaunchConfiguration('gui'))),
+
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz2',
+            output='screen'),
+    ])
diff --git a/iai_stacy_description/launch/hardware.launch.py b/iai_stacy_description/launch/hardware.launch.py
@@ -0,0 +1,52 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.substitutions import Command, LaunchConfiguration
+from launch_ros.actions import Node
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory
+import os
+
+def generate_launch_description():
+    # Launch args for IPs
+    ip = LaunchConfiguration('robot_ip')
+
+    orbbec_share_dir = get_package_share_directory('orbbec_camera')
+    launch_dir = os.path.join(orbbec_share_dir, 'launch')
+
+    return LaunchDescription([
+        DeclareLaunchArgument('robot_ip', default_value='192.168.1.2'),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([
+                os.path.join(
+                    get_package_share_directory('ur_robot_driver'),
+                    'launch',
+                    'ur_control.launch.py'
+                )
+            ]),
+            launch_arguments={
+                'robot_ip': ip,
+                'use_fake_hardware': 'false',
+                'ur_type': 'ur5',
+                'initial_joint_controller': 'forward_velocity_controller',
+                'launch_rviz': 'false',
+                'description_launchfile': os.path.join(
+                    get_package_share_directory('iai_stacy_description'),
+                    'launch',
+                    'rsp.launch.py'
+                ),
+            }.items()
+        ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(launch_dir, 'femto_bolt.launch.py')
+            ),
+            launch_arguments={
+                'color_width': '1920',
+                'color_height': '1080',
+                'depth_registration': 'True',
+                'enable_colored_point_cloud': 'True',
+                'enable_noise_removal_filter': 'True',
+                'noise_removal_filter_min_diff': '3'
+            }.items()
+        ),
+    ])
diff --git a/iai_stacy_description/launch/rsp.launch.py b/iai_stacy_description/launch/rsp.launch.py
@@ -0,0 +1,49 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    ur_type = LaunchConfiguration('ur_type')
+    robot_ip = LaunchConfiguration('robot_ip')
+    kinematics_parameters_file = LaunchConfiguration('kinematics_parameters_file')
+    use_mock_hardware = LaunchConfiguration('use_mock_hardware')
+    mock_sensor_commands = LaunchConfiguration('mock_sensor_commands')
+    headless_mode = LaunchConfiguration('headless_mode')
+
+    robot_description_content = Command([
+        PathJoinSubstitution([FindExecutable(name='xacro')]),
+        ' ',
+        PathJoinSubstitution([FindPackageShare('iai_stacy_description'), 'urdf', 'stacy_controlled.urdf.xacro']),
+        ' robot_ip:=', robot_ip,
+        ' ur_type:=', ur_type,
+        ' kinematics_parameters_file:=', kinematics_parameters_file,
+        ' use_mock_hardware:=', use_mock_hardware,
+        ' mock_sensor_commands:=', mock_sensor_commands,
+        ' headless_mode:=', headless_mode,
+    ])
+
+    return LaunchDescription([
+        DeclareLaunchArgument('ur_type', default_value='ur5',
+                              description='UR robot type'),
+        DeclareLaunchArgument('robot_ip',
+                              description='IP address of the UR robot'),
+        DeclareLaunchArgument(
+            'kinematics_parameters_file',
+            default_value=PathJoinSubstitution([
+                FindPackageShare('ur_description'), 'config', ur_type, 'default_kinematics.yaml'
+            ]),
+            description='Kinematics calibration file (use robot-specific file for real hardware)'),
+        DeclareLaunchArgument('use_mock_hardware', default_value='false',
+                              description='Use mock hardware instead of real robot'),
+        DeclareLaunchArgument('mock_sensor_commands', default_value='false'),
+        DeclareLaunchArgument('headless_mode', default_value='false'),
+
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            output='both',
+            parameters=[{'robot_description': robot_description_content}]),
+    ])
diff --git a/iai_stacy_description/meshes/camera_adapter.stl b/iai_stacy_description/meshes/camera_adapter.stl
diff --git a/iai_stacy_description/meshes/item_robot_column.stl b/iai_stacy_description/meshes/item_robot_column.stl
diff --git a/iai_stacy_description/package.xml b/iai_stacy_description/package.xml
@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>iai_stacy_description</name>
+  <version>1.0.0</version>
+  <description>URDF description and bringup launch files for the Stacy robot at IAI, University of Bremen.</description>
+
+  <maintainer email="huerkamp@uni-bremen.de">Malte Huerkamp</maintainer>
+
+  <license>LGPL</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>xacro</build_depend>
+  <exec_depend>xacro</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>ur_description</exec_depend>
+  <exec_depend>ur_robot_driver</exec_depend>
+  <exec_depend>orbbec_description</exec_depend>
+  <exec_depend>orbbec_camera</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/iai_stacy_description/urdf/stacy.urdf.xacro b/iai_stacy_description/urdf/stacy.urdf.xacro
@@ -0,0 +1,24 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro" name="stacy">
+
+  <xacro:include filename="$(find iai_stacy_description)/urdf/stacy_macro.xacro"/>
+
+  <xacro:arg name="ur_type" default="ur5"/>
+  <xacro:arg name="joint_limits_parameters_file"  default="$(find ur_description)/config/$(arg ur_type)/joint_limits.yaml"/>
+  <xacro:arg name="kinematics_parameters_file"    default="$(find ur_description)/config/$(arg ur_type)/default_kinematics.yaml"/>
+  <xacro:arg name="physical_parameters_file"      default="$(find ur_description)/config/$(arg ur_type)/physical_parameters.yaml"/>
+  <xacro:arg name="visual_parameters_file"        default="$(find ur_description)/config/$(arg ur_type)/visual_parameters.yaml"/>
+
+  <link name="map"/>
+
+  <xacro:stacy
+    parent="map"
+    ur_type="$(arg ur_type)"
+    joint_limits_parameters_file="$(arg joint_limits_parameters_file)"
+    kinematics_parameters_file="$(arg kinematics_parameters_file)"
+    physical_parameters_file="$(arg physical_parameters_file)"
+    visual_parameters_file="$(arg visual_parameters_file)">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </xacro:stacy>
+
+</robot>
diff --git a/iai_stacy_description/urdf/stacy_controlled.urdf.xacro b/iai_stacy_description/urdf/stacy_controlled.urdf.xacro
@@ -0,0 +1,44 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro" name="stacy">
+
+  <xacro:include filename="$(find iai_stacy_description)/urdf/stacy_macro.xacro"/>
+  <xacro:include filename="$(find ur_robot_driver)/urdf/ur.ros2_control.xacro"/>
+
+  <xacro:arg name="ur_type"                    default="ur5"/>
+  <xacro:arg name="robot_ip"                   default="192.168.1.2"/>
+  <xacro:arg name="kinematics_parameters_file" default="$(find ur_description)/config/$(arg ur_type)/default_kinematics.yaml"/>
+  <xacro:arg name="joint_limits_parameters_file"  default="$(find ur_description)/config/$(arg ur_type)/joint_limits.yaml"/>
+  <xacro:arg name="physical_parameters_file"      default="$(find ur_description)/config/$(arg ur_type)/physical_parameters.yaml"/>
+  <xacro:arg name="visual_parameters_file"        default="$(find ur_description)/config/$(arg ur_type)/visual_parameters.yaml"/>
+  <xacro:arg name="use_mock_hardware"          default="false"/>
+  <xacro:arg name="mock_sensor_commands"       default="false"/>
+  <xacro:arg name="headless_mode"              default="false"/>
+  <xacro:arg name="script_filename"            default="$(find ur_client_library)/resources/external_control.urscript"/>
+  <xacro:arg name="input_recipe_filename"      default="$(find ur_robot_driver)/resources/rtde_input_recipe.txt"/>
+  <xacro:arg name="output_recipe_filename"     default="$(find ur_robot_driver)/resources/rtde_output_recipe.txt"/>
+
+  <link name="map"/>
+
+  <xacro:stacy
+    parent="map"
+    ur_type="$(arg ur_type)"
+    joint_limits_parameters_file="$(arg joint_limits_parameters_file)"
+    kinematics_parameters_file="$(arg kinematics_parameters_file)"
+    physical_parameters_file="$(arg physical_parameters_file)"
+    visual_parameters_file="$(arg visual_parameters_file)">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </xacro:stacy>
+
+  <xacro:ur_ros2_control
+    name="ur"
+    tf_prefix=""
+    kinematics_parameters_file="$(arg kinematics_parameters_file)"
+    robot_ip="$(arg robot_ip)"
+    script_filename="$(arg script_filename)"
+    input_recipe_filename="$(arg input_recipe_filename)"
+    output_recipe_filename="$(arg output_recipe_filename)"
+    use_mock_hardware="$(arg use_mock_hardware)"
+    mock_sensor_commands="$(arg mock_sensor_commands)"
+    headless_mode="$(arg headless_mode)"/>
+
+</robot>