diff --git a/launch/robot_nodes_launch.py b/launch/robot_nodes_launch.py
new file mode 100644
index 0000000..84b8886
--- /dev/null
+++ b/launch/robot_nodes_launch.py
@@ -0,0 +1,131 @@
+#!/usr/bin/env python
+
+# Copyright 1996-2023 Cyberbotics Ltd.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Launch Webots Universal Robot simulation nodes."""
+
+import os
+import launch
+from launch_ros.actions import Node
+from launch import LaunchDescription
+from ament_index_python.packages import get_package_share_directory
+from webots_ros2_driver.urdf_spawner import URDFSpawner, get_webots_driver_node
+from webots_ros2_driver.webots_controller import WebotsController
+from rbs_utils.launch import load_xacro_args
+from robot_builder.external.ros2_control import ControllerManager
+from robot_builder.parser.urdf import URDF_parser
+import xacro
+
+
+PACKAGE_NAME = 'aubo_description'
+
+
+def generate_launch_description():
+ package_dir = get_package_share_directory(PACKAGE_NAME)
+ robot_description_path = os.path.join(package_dir, 'urdf', 'aubo_i5.urdf.xacro')
+ controllers_file = os.path.join(package_dir, 'config', 'controllers.yaml')
+
+ mappings_data = load_xacro_args(robot_description_path, locals())
+
+ robot_description_doc = xacro.process_file(
+ robot_description_path, mappings={"robot_ip": "", "simulation_controllers": controllers_file, "hardware": "webots", "with_gripper": "false"}
+ )
+
+ robot_description_content = robot_description_doc.toprettyxml(indent=" ")
+ # with open("current.urdf", "w") as file:
+ # file.write(robot_description_content)
+ # file.close()
+
+ robot = URDF_parser.load_string(
+ robot_description_content,
+ base_link_name="base_link",
+ ee_link_name="wrist3_Link",
+ )
+ ControllerManager.save_to_yaml(
+ robot, package_dir, "controllers.yaml"
+ )
+
+ # Define your URDF robots here
+ # The name of an URDF robot has to match the name of the robot of the driver node
+ # You can specify the URDF file to use with "urdf_path"
+ spawn_URDF_aubo = URDFSpawner(
+ name='Aubo-i5',
+ robot_description=robot_description_content,
+ translation='0 0 0.6',
+ rotation='0 0 1 -1.5708',
+ )
+
+ # Driver nodes
+ # When having multiple robot it is mandatory to specify the robot name.
+ aubo_robot_driver = WebotsController(
+ robot_name='Aubo-i5',
+ parameters=[
+ {'robot_description': robot_description_content},
+ {'use_sim_time': True},
+ {'set_robot_state_publisher': True},
+ controllers_file
+ ],
+ )
+
+ # Other ROS 2 nodes
+ controller_manager_timeout = ['--controller-manager-timeout', '100']
+ trajectory_controller_spawner = Node(
+ package='controller_manager',
+ executable='spawner',
+ output='screen',
+ arguments=['cartesian_motion_controller'] + controller_manager_timeout,
+ )
+
+ joint_state_broadcaster_spawner = Node(
+ package='controller_manager',
+ executable='spawner',
+ output='screen',
+ arguments=['joint_state_broadcaster'] + controller_manager_timeout,
+ )
+
+ robot_state_publisher = Node(
+ package='robot_state_publisher',
+ executable='robot_state_publisher',
+ output='screen',
+ parameters=[{
+ 'robot_description': ''
+ }],
+ )
+
+ return LaunchDescription([
+ # Request to spawn the URDF robot
+ spawn_URDF_aubo,
+
+ # Other ROS 2 nodes
+ robot_state_publisher,
+ trajectory_controller_spawner,
+ joint_state_broadcaster_spawner,
+
+ # Launch the driver node once the URDF robot is spawned
+ launch.actions.RegisterEventHandler(
+ event_handler=launch.event_handlers.OnProcessIO(
+ target_action=spawn_URDF_aubo,
+ on_stdout=lambda event: get_webots_driver_node(event, aubo_robot_driver),
+ )
+ ),
+
+ # Kill all the nodes when the driver node is shut down
+ launch.actions.RegisterEventHandler(
+ event_handler=launch.event_handlers.OnProcessExit(
+ target_action=aubo_robot_driver,
+ on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
+ )
+ ),
+ ])
diff --git a/launch/robot_world_launch.py b/launch/robot_world_launch.py
new file mode 100644
index 0000000..e85d76e
--- /dev/null
+++ b/launch/robot_world_launch.py
@@ -0,0 +1,55 @@
+#!/usr/bin/env python
+
+# Copyright 1996-2023 Cyberbotics Ltd.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Launch Webots Universal Robot simulation world."""
+
+import launch
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.substitutions.path_join_substitution import PathJoinSubstitution
+from webots_ros2_driver.webots_launcher import WebotsLauncher
+
+
+PACKAGE_NAME = 'aubo_description'
+
+
+def generate_launch_description():
+ package_dir = get_package_share_directory(PACKAGE_NAME)
+ world = LaunchConfiguration('world')
+
+ # Starts Webots
+ webots = WebotsLauncher(
+ world=PathJoinSubstitution([package_dir, 'world', world]),
+ ros2_supervisor=True
+ )
+
+ return LaunchDescription([
+ DeclareLaunchArgument(
+ 'world',
+ default_value='aubo_robot.wbt',
+ ),
+ webots,
+ webots._supervisor,
+ # This action will kill all nodes once the Webots simulation has exited
+ launch.actions.RegisterEventHandler(
+ event_handler=launch.event_handlers.OnProcessExit(
+ target_action=webots,
+ on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
+ )
+ )
+ ])
diff --git a/urdf/aubo_i5.urdf b/urdf/aubo_i5.urdf
index c0e9e9a..1143777 100755
--- a/urdf/aubo_i5.urdf
+++ b/urdf/aubo_i5.urdf
@@ -1,220 +1,275 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ webots_ros2_control::Ros2ControlSystem
+
+
+
+
+
+ 0.0
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.0
+
+
+
+
+
+
+
+
diff --git a/urdf/aubo_i5.urdf.xacro b/urdf/aubo_i5.urdf.xacro
index d099f34..732b04c 100644
--- a/urdf/aubo_i5.urdf.xacro
+++ b/urdf/aubo_i5.urdf.xacro
@@ -1,9 +1,4 @@
-
-
-
-
-
@@ -243,19 +238,12 @@
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
diff --git a/urdf/ros2_control.xacro b/urdf/ros2_control.xacro
index 2827d67..94c1062 100644
--- a/urdf/ros2_control.xacro
+++ b/urdf/ros2_control.xacro
@@ -13,6 +13,9 @@
aubo_driver/AuboHardwareInterface
${robot_ip}
+
+ webots_ros2_control::Ros2ControlSystem
+
@@ -35,7 +38,7 @@
0.5
-
+
@@ -51,7 +54,7 @@
1.75
-
+
@@ -66,7 +69,7 @@
0.0
-
+
@@ -81,7 +84,7 @@
1.57
-
+
@@ -91,7 +94,7 @@
-
+
0.0
diff --git a/urdf/xacro_args.yaml b/urdf/xacro_args.yaml
index 51557a8..eaa728f 100644
--- a/urdf/xacro_args.yaml
+++ b/urdf/xacro_args.yaml
@@ -1,4 +1,4 @@
simulation_controllers: !variable controllers_file
robot_ip: 192.168.0.106
-hardware: gazebo
+hardware: webots
with_gripper: "true"
diff --git a/world/aubo_robot.wbt b/world/aubo_robot.wbt
new file mode 100644
index 0000000..d3e7b99
--- /dev/null
+++ b/world/aubo_robot.wbt
@@ -0,0 +1,49 @@
+#VRML_SIM R2023b utf8
+
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/backgrounds/protos/TexturedBackground.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/floors/protos/Floor.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/appearances/protos/ThreadMetalPlate.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/objects/solids/protos/SolidBox.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/develop/projects/appearances/protos/GalvanizedMetal.proto"
+
+WorldInfo {
+ info [
+ "Aubo i5 Robot"
+ ]
+ title "Aubo Robot"
+ basicTimeStep 16
+ physicsDisableAngularThreshold 0.1
+ contactProperties [
+ ContactProperties {
+ bounce 0.1
+ }
+ ContactProperties {
+ material1 "slope"
+ coulombFriction [
+ 0.5
+ ]
+ }
+ ]
+}
+Viewpoint {
+ orientation 0.3118460054718159 0.06120895855528569 -0.9481590226664772 2.710827783218323
+ position 2.9308358456729615 1.1235070287382412 2.814381048353761
+ exposure 1.7
+}
+TexturedBackground {
+ texture "factory"
+}
+Floor {
+ size 20 20
+ appearance ThreadMetalPlate {
+ }
+}
+SolidBox {
+ translation 0 0 0.3
+ size 0.3 0.3 0.6
+ appearance GalvanizedMetal {
+ textureTransform TextureTransform {
+ scale 2 2
+ }
+ }
+}