diff --git a/CustomRobots/robot_arms/config/ur3_controllers.yaml b/CustomRobots/robot_arms/config/ur3_controllers.yaml
new file mode 100644
index 000000000..0f2d6d906
--- /dev/null
+++ b/CustomRobots/robot_arms/config/ur3_controllers.yaml
@@ -0,0 +1,86 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    gripper_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+joint_state_broadcaster:
+  ros__parameters:
+    joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+      - robotiq_85_left_knuckle_joint
+
+    interfaces:
+      - position
+      - velocity
+      - effort
+
+joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+
+    command_interfaces:
+      - position
+
+    state_interfaces:
+      - position
+      - velocity
+
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    allow_nonzero_velocity_at_trajectory_end: true
+    allow_integration_in_goal_trajectories: true
+
+    constraints:
+      stopped_velocity_tolerance: 0.01
+      goal_time: 0.0
+
+gripper_controller:
+  ros__parameters:
+    joints:
+      - robotiq_85_left_knuckle_joint
+
+    command_interfaces:
+      - position
+
+    state_interfaces:
+      - position
+      - velocity
+
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    allow_integration_in_goal_trajectories: true
+
+    constraints:
+      stopped_velocity_tolerance: 0.01
+      goal_time: 0.0
+      robotiq_85_left_knuckle_joint:
+        trajectory: 0.5
+        goal: 0.5
+
+    gains:
+      robotiq_85_left_knuckle_joint:
+        p: 100.0
+        d: 1.0
+        i: 0.01
+        i_clamp: 1.0
diff --git a/CustomRobots/robot_arms/config/ur5/joint_limits.yaml b/CustomRobots/robot_arms/config/ur5/joint_limits.yaml
index e6093ea65..947574c93 100644
--- a/CustomRobots/robot_arms/config/ur5/joint_limits.yaml
+++ b/CustomRobots/robot_arms/config/ur5/joint_limits.yaml
@@ -15,9 +15,9 @@ joint_limits:
     has_position_limits: true
     has_velocity_limits: true
     max_effort: 150.0
-    max_position: !degrees  360.0
+    max_position: !degrees  180.0
     max_velocity: !degrees  180.0
-    min_position: !degrees -360.0
+    min_position: !degrees -180.0
   shoulder_lift_joint:
     # acceleration limits are not publicly available
     has_acceleration_limits: false
@@ -25,9 +25,9 @@ joint_limits:
     has_position_limits: true
     has_velocity_limits: true
     max_effort: 150.0
-    max_position: !degrees  360.0
+    max_position: !degrees  180.0
     max_velocity: !degrees  180.0
-    min_position: !degrees -360.0
+    min_position: !degrees -180.0
   elbow_joint:
     # acceleration limits are not publicly available
     has_acceleration_limits: false
@@ -55,9 +55,9 @@ joint_limits:
     has_position_limits: true
     has_velocity_limits: true
     max_effort: 28.0
-    max_position: !degrees  360.0
+    max_position: !degrees  180.0
     max_velocity: !degrees  180.0
-    min_position: !degrees -360.0
+    min_position: !degrees -180.0
   wrist_2_joint:
     # acceleration limits are not publicly available
     has_acceleration_limits: false
@@ -65,9 +65,9 @@ joint_limits:
     has_position_limits: true
     has_velocity_limits: true
     max_effort: 28.0
-    max_position: !degrees  360.0
+    max_position: !degrees  180.0
     max_velocity: !degrees  180.0
-    min_position: !degrees -360.0
+    min_position: !degrees -180.0
   wrist_3_joint:
     # acceleration limits are not publicly available
     has_acceleration_limits: false
@@ -75,6 +75,6 @@ joint_limits:
     has_position_limits: true
     has_velocity_limits: true
     max_effort: 28.0
-    max_position: !degrees  360.0
+    max_position: !degrees  180.0
     max_velocity: !degrees  180.0
-    min_position: !degrees -360.0
+    min_position: !degrees -180.0
diff --git a/CustomRobots/robot_arms/launch/ur3.launch.py b/CustomRobots/robot_arms/launch/ur3.launch.py
new file mode 100644
index 000000000..7c5aa7cd3
--- /dev/null
+++ b/CustomRobots/robot_arms/launch/ur3.launch.py
@@ -0,0 +1,351 @@
+from launch.actions import (
+    IncludeLaunchDescription,
+    DeclareLaunchArgument,
+    SetEnvironmentVariable,
+)
+from launch.substitutions import LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+import os
+import xacro
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+
+
+def load_file(package_name, file_path):
+    pkg_path = get_package_share_directory(package_name)
+    with open(os.path.join(pkg_path, file_path), "r") as f:
+        return f.read()
+
+
+def load_yaml(package_name, file_path):
+    pkg_path = get_package_share_directory(package_name)
+    with open(os.path.join(pkg_path, file_path), "r") as f:
+        return yaml.safe_load(f)
+
+
+def launch_setup(context):
+    x = LaunchConfiguration("x")
+    y = LaunchConfiguration("y")
+    z = LaunchConfiguration("z")
+    R = LaunchConfiguration("R")
+    P = LaunchConfiguration("P")
+    Y = LaunchConfiguration("Y")
+    gz_sensor = LaunchConfiguration("sensor")
+
+    package_dir = get_package_share_directory("custom_robots")
+
+    sensor = gz_sensor.perform(context)
+
+    nodes = []
+
+    # =========================
+    # ROBOT DESCRIPTION (URDF)
+    # =========================
+    xacro_file = os.path.join(
+        package_dir,
+        "models",
+        "ur3",
+        "ur3.urdf.xacro",
+    )
+
+    controllers_file = os.path.join(package_dir, "config", "ur3_controllers.yaml")
+
+    robot_description_content = xacro.process_file(
+        xacro_file,
+        mappings={
+            "ur_type": "ur3",
+            "name": "ur",
+            "prefix": "",
+            "use_fake_hardware": "false",
+            "sim_gazebo": "false",
+            "sim_gz": "true",
+            "simulation_controllers": controllers_file,
+            "hmi": "false",
+            "EE": "true",
+            "EE_name": "robotiq_2f85",
+            "camera": "true" if sensor == "camera" else "false",
+        },
+    ).toxml()
+
+    robot_description = {"robot_description": robot_description_content}
+
+    # =========================
+    # SRDF (SEMANTIC)
+    # =========================
+    robot_description_semantic = {
+        "robot_description_semantic": load_file(
+            "ros2srrc_ur3_moveit2", "config/ur3robotiq_2f85.srdf"
+        )
+    }
+
+    # =========================
+    # KINEMATICS
+    # =========================
+    kinematics_yaml = load_yaml("ur3_gripper_moveit_config", "config/kinematics.yaml")
+
+    kinematics_yaml = {
+        "robot_description_kinematics": kinematics_yaml["/**"]["ros__parameters"]
+    }
+
+    # =========================
+    # CONTROLLERS (MoveIt)
+    # =========================
+    moveit_controllers = load_yaml(
+        "ur3_gripper_moveit_config", "config/moveit_controllers.yaml"
+    )
+
+    ompl_planning = load_yaml("ur3_gripper_moveit_config", "config/ompl_planning.yaml")
+
+    ompl_planning = ompl_planning["/**"]["ros__parameters"]
+
+    moveit_controllers = moveit_controllers["/**"]["ros__parameters"]
+
+    planning_pipelines_config = {
+        "planning_pipelines": ["ompl", "pilz_industrial_motion_planner"],
+        "default_planning_pipeline": "pilz_industrial_motion_planner",
+        "ompl": {
+            "planning_plugin": "ompl_interface/OMPLPlanner",
+        },
+        "pilz_industrial_motion_planner": {
+            "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner",
+            "request_adapters": "",
+            "start_state_max_bounds_error": 0.1,
+            "default_planner_config": "PTP",
+        },
+    }
+
+    move_group_capabilities = {
+        "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction "
+        "pilz_industrial_motion_planner/MoveGroupSequenceService"
+    }
+
+    pilz_cartesian_limits = load_yaml(
+        "ros2srrc_robots", "ur3/config/pilz_cartesian_limits.yaml"
+    )
+
+    joint_limits_yaml = load_yaml("ros2srrc_robots", "ur3/config/joint_limits.yaml")
+
+    combined_planning = {
+        "robot_description_planning": {**joint_limits_yaml, **pilz_cartesian_limits}
+    }
+
+    # =========================
+    # NODES
+    # =========================
+
+    move = Node(
+        package="ros2srrc_execution",
+        executable="move",
+        name="move_action_server",
+        output="screen",
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            kinematics_yaml,
+            moveit_controllers,
+            ompl_planning,
+            {
+                "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager"
+            },
+            {"use_sim_time": True},
+            {"ROB_PARAM": "ur3"},
+            {"EE_PARAM": "robotiq_2f85"},
+            {"ROB_GROUP": "ur3_arm"},
+        ],
+    )
+
+    robmove = Node(
+        package="ros2srrc_execution",
+        executable="robmove",
+        name="Robmove",
+        output="screen",
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            kinematics_yaml,
+            moveit_controllers,
+            ompl_planning,
+            {
+                "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager"
+            },
+            {"use_sim_time": True},
+            {"ROB_PARAM": "ur3"},
+            {"ROB_GROUP": "ur3_arm"},
+        ],
+    )
+
+    robpose = Node(
+        package="ros2srrc_execution",
+        executable="robpose",
+        name="robpose",
+        output="screen",
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            kinematics_yaml,
+            ompl_planning,
+            {"use_sim_time": True},
+            {"ROB_PARAM": "ur3"},
+            {"ROB_GROUP": "ur3_arm"},
+        ],
+    )
+
+    nodes.append(move)
+    nodes.append(robmove)
+    nodes.append(robpose)
+
+    # =========================
+    # CORE NODES
+    # =========================
+
+    robot_state_publisher = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        parameters=[robot_description, {"use_sim_time": True}],
+        output="screen",
+    )
+
+    static_tf = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        arguments=[x, y, z, R, P, Y, "world", "base_link"],
+    )
+
+    spawn_robot = Node(
+        package="ros_gz_sim",
+        executable="create",
+        arguments=[
+            "-topic",
+            "robot_description",
+            "-name",
+            "ur3_robotiq",
+            "-x",
+            x,
+            "-y",
+            y,
+            "-z",
+            z,
+            "-R",
+            R,
+            "-P",
+            P,
+            "-Y",
+            Y,
+        ],
+        output="screen",
+    )
+
+    clock_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock]"],
+        parameters=[{"use_sim_time": True}],
+    )
+
+    nodes.append(robot_state_publisher)
+    nodes.append(static_tf)
+    nodes.append(spawn_robot)
+    nodes.append(clock_bridge)
+
+    if sensor == "camera":
+        camera_bridge = Node(
+            package="ros_gz_bridge",
+            executable="parameter_bridge",
+            arguments=[
+                "/hand_camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo",
+                "/base_camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo",
+            ],
+            output="screen",
+        )
+
+        gz_ros2_image_bridge = Node(
+            package="ros_gz_image",
+            executable="image_bridge",
+            arguments=["/hand_camera/image"],
+            output="screen",
+        )
+
+        gz_ros2_base_image_bridge = Node(
+            package="ros_gz_image",
+            executable="image_bridge",
+            arguments=["/base_camera/image"],
+            output="screen",
+        )
+
+        nodes.append(camera_bridge)
+        nodes.append(gz_ros2_image_bridge)
+        nodes.append(gz_ros2_base_image_bridge)
+
+    # =========================
+    # CONTROLLERS
+    # =========================
+
+    joint_state_broadcaster = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster"],
+    )
+
+    joint_trajectory_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_trajectory_controller"],
+    )
+
+    gripper_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["gripper_controller"],
+    )
+
+    nodes.append(joint_state_broadcaster)
+    nodes.append(joint_trajectory_controller)
+    nodes.append(gripper_controller)
+
+    # =========================
+    # MOVEIT
+    # =========================
+
+    move_group = Node(
+        package="moveit_ros_move_group",
+        executable="move_group",
+        output="screen",
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            kinematics_yaml,
+            planning_pipelines_config,
+            move_group_capabilities,
+            moveit_controllers,
+            combined_planning,
+            {"use_sim_time": True},
+        ],
+    )
+
+    nodes.append(move_group)
+
+    # =========================
+    # LAUNCH
+    # =========================
+
+    return nodes
+
+
+def generate_launch_description():
+    declared_arguments = [
+        DeclareLaunchArgument("x", default_value="0"),
+        DeclareLaunchArgument("y", default_value="0"),
+        DeclareLaunchArgument("z", default_value="0.9"),
+        DeclareLaunchArgument("R", default_value="0"),
+        DeclareLaunchArgument("P", default_value="0"),
+        DeclareLaunchArgument("Y", default_value="0"),
+        DeclareLaunchArgument("sensor", default_value="none"),
+    ]
+
+    return LaunchDescription(
+        declared_arguments + [OpaqueFunction(function=launch_setup)]
+    )
diff --git a/CustomRobots/robot_arms/launch/ur5.launch.py b/CustomRobots/robot_arms/launch/ur5.launch.py
index 6668c5434..b18b06b15 100644
--- a/CustomRobots/robot_arms/launch/ur5.launch.py
+++ b/CustomRobots/robot_arms/launch/ur5.launch.py
@@ -154,6 +154,7 @@ def launch_setup(context):
             {"use_sim_time": True},
             {"ROB_PARAM": "ur5"},
             {"EE_PARAM": "robotiq_2f85"},
+            {"ROB_GROUP": "ur5_manipulator"},
         ],
     )
 
@@ -173,6 +174,7 @@ def launch_setup(context):
             },
             {"use_sim_time": True},
             {"ROB_PARAM": "ur5"},
+            {"ROB_GROUP": "ur5_manipulator"},
         ],
     )
 
@@ -188,6 +190,7 @@ def launch_setup(context):
             ompl_planning,
             {"use_sim_time": True},
             {"ROB_PARAM": "ur5"},
+            {"ROB_GROUP": "ur5_manipulator"},
         ],
     )
 
diff --git a/CustomRobots/robot_arms/models/ur3/inc/ur_common.xacro b/CustomRobots/robot_arms/models/ur3/inc/ur_common.xacro
new file mode 100644
index 000000000..4281d0033
--- /dev/null
+++ b/CustomRobots/robot_arms/models/ur3/inc/ur_common.xacro
@@ -0,0 +1,208 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro">
+  <!--
+    NOTE the macros defined in this file are NOT part of the public API of this
+         package. Users CANNOT rely on this file being available, or stored in
+         this location. Nor can they rely on the existence of the two macros.
+         The macros store the defined properties in the scope of the caller.
+         However, users MUST NOT rely on these properties, their contents or their
+         names.
+
+    Author: Ludovic Delval
+    Contributor: Felix Messmer
+  -->
+  <xacro:macro name="cylinder_inertial" params="radius length mass *origin">
+    <inertial>
+      <mass value="${mass}" />
+      <xacro:insert_block name="origin" />
+      <inertia ixx="${0.0833333 * mass * (3 * radius * radius + length * length)}" ixy="0.0" ixz="0.0"
+        iyy="${0.0833333 * mass * (3 * radius * radius + length * length)}" iyz="0.0"
+        izz="${0.5 * mass * radius * radius}" />
+    </inertial>
+  </xacro:macro>
+
+  <xacro:macro name="get_visual_params" params="name:=^ type:=^" >
+    <xacro:property name="visual_params" value="${sec_mesh_files[name][type]}" scope="parent"/>
+  </xacro:macro>
+
+  <!-- Simplification of getting meshes. Available types can be seen in the visual_parameters.yaml (At the time of writing: visual, collision) -->
+  <xacro:macro name="get_mesh_path" params="name:=^ type:=^" >
+    <xacro:get_visual_params />
+    <xacro:if value="${force_abs_paths}">
+      <xacro:property name="mesh" value="file://$(find ${visual_params['mesh']['package']})/${visual_params['mesh']['path']}" scope="parent"/>
+    </xacro:if>
+    <xacro:unless value="${force_abs_paths}">
+      <xacro:property name="mesh" value="package://${visual_params['mesh']['package']}/${visual_params['mesh']['path']}" scope="parent"/>
+    </xacro:unless>
+  </xacro:macro>
+
+  <xacro:macro name="get_mesh" params="name type" >
+    <xacro:get_mesh_path/>
+    <mesh filename="${mesh}"/>
+  </xacro:macro>
+
+  <xacro:macro name="read_model_data" params="joint_limits_parameters_file kinematics_parameters_file physical_parameters_file visual_parameters_file force_abs_paths">
+
+    <xacro:property name="force_abs_paths" value="${force_abs_paths}" scope="parent"/>
+    <!-- Read .yaml files from disk, load content into properties -->
+    <xacro:property name="config_joint_limit_parameters" value="${load_yaml(joint_limits_parameters_file)}"/>
+    <xacro:property name="config_kinematics_parameters" value="${load_yaml(kinematics_parameters_file)}"/>
+    <xacro:property name="config_physical_parameters" value="${load_yaml(physical_parameters_file)}"/>
+    <xacro:property name="config_visual_parameters" value="${load_yaml(visual_parameters_file)}"/>
+
+    <!-- Extract subsections from yaml dictionaries -->
+    <xacro:property name="sec_limits" value="${config_joint_limit_parameters['joint_limits']}"/>
+    <xacro:property name="sec_dh_parameters" value="${config_physical_parameters['dh_parameters']}"/>
+    <xacro:property name="sec_offsets" value="${config_physical_parameters['offsets']}"/>
+    <xacro:property name="sec_inertia_parameters" value="${config_physical_parameters['inertia_parameters']}" />
+    <xacro:property name="sec_mesh_files" value="${config_visual_parameters['mesh_files']}" scope="parent"/>
+    <xacro:property name="sec_kinematics" value="${config_kinematics_parameters['kinematics']}" />
+
+    <!-- JOINTS LIMIT PARAMETERS -->
+    <xacro:property name="shoulder_pan_lower_limit" value="${sec_limits['shoulder_pan_joint']['min_position']}" scope="parent"/>
+    <xacro:property name="shoulder_pan_upper_limit" value="${sec_limits['shoulder_pan_joint']['max_position']}" scope="parent"/>
+    <xacro:property name="shoulder_pan_velocity_limit" value="${sec_limits['shoulder_pan_joint']['max_velocity']}" scope="parent"/>
+    <xacro:property name="shoulder_pan_effort_limit" value="${sec_limits['shoulder_pan_joint']['max_effort']}" scope="parent"/>
+    <xacro:property name="shoulder_lift_lower_limit" value="${sec_limits['shoulder_lift_joint']['min_position']}" scope="parent"/>
+    <xacro:property name="shoulder_lift_upper_limit" value="${sec_limits['shoulder_lift_joint']['max_position']}" scope="parent"/>
+    <xacro:property name="shoulder_lift_velocity_limit" value="${sec_limits['shoulder_lift_joint']['max_velocity']}" scope="parent"/>
+    <xacro:property name="shoulder_lift_effort_limit" value="${sec_limits['shoulder_lift_joint']['max_effort']}" scope="parent"/>
+    <xacro:property name="elbow_joint_lower_limit" value="${sec_limits['elbow_joint']['min_position']}" scope="parent"/>
+    <xacro:property name="elbow_joint_upper_limit" value="${sec_limits['elbow_joint']['max_position']}" scope="parent"/>
+    <xacro:property name="elbow_joint_velocity_limit" value="${sec_limits['elbow_joint']['max_velocity']}" scope="parent"/>
+    <xacro:property name="elbow_joint_effort_limit" value="${sec_limits['elbow_joint']['max_effort']}" scope="parent"/>
+    <xacro:property name="wrist_1_lower_limit" value="${sec_limits['wrist_1_joint']['min_position']}" scope="parent"/>
+    <xacro:property name="wrist_1_upper_limit" value="${sec_limits['wrist_1_joint']['max_position']}" scope="parent"/>
+    <xacro:property name="wrist_1_velocity_limit" value="${sec_limits['wrist_1_joint']['max_velocity']}" scope="parent"/>
+    <xacro:property name="wrist_1_effort_limit" value="${sec_limits['wrist_1_joint']['max_effort']}" scope="parent"/>
+    <xacro:property name="wrist_2_lower_limit" value="${sec_limits['wrist_2_joint']['min_position']}" scope="parent"/>
+    <xacro:property name="wrist_2_upper_limit" value="${sec_limits['wrist_2_joint']['max_position']}" scope="parent"/>
+    <xacro:property name="wrist_2_velocity_limit" value="${sec_limits['wrist_2_joint']['max_velocity']}" scope="parent"/>
+    <xacro:property name="wrist_2_effort_limit" value="${sec_limits['wrist_2_joint']['max_effort']}" scope="parent"/>
+    <xacro:property name="wrist_3_lower_limit" value="${sec_limits['wrist_3_joint']['min_position']}" scope="parent"/>
+    <xacro:property name="wrist_3_upper_limit" value="${sec_limits['wrist_3_joint']['max_position']}" scope="parent"/>
+    <xacro:property name="wrist_3_velocity_limit" value="${sec_limits['wrist_3_joint']['max_velocity']}" scope="parent"/>
+    <xacro:property name="wrist_3_effort_limit" value="${sec_limits['wrist_3_joint']['max_effort']}" scope="parent"/>
+
+    <!-- DH PARAMETERS -->
+    <xacro:property name="d1" value="${sec_dh_parameters['d1']}" scope="parent"/>
+    <xacro:property name="a2" value="${sec_dh_parameters['a2']}" scope="parent"/>
+    <xacro:property name="a3" value="${sec_dh_parameters['a3']}" scope="parent"/>
+    <xacro:property name="d4" value="${sec_dh_parameters['d4']}" scope="parent"/>
+    <xacro:property name="d5" value="${sec_dh_parameters['d5']}" scope="parent"/>
+    <xacro:property name="d6" value="${sec_dh_parameters['d6']}" scope="parent"/>
+
+    <!-- kinematics -->
+    <xacro:property name="shoulder_x" value="${sec_kinematics['shoulder']['x']}" scope="parent"/>
+    <xacro:property name="shoulder_y" value="${sec_kinematics['shoulder']['y']}" scope="parent"/>
+    <xacro:property name="shoulder_z" value="${sec_kinematics['shoulder']['z']}" scope="parent"/>
+    <xacro:property name="shoulder_roll" value="${sec_kinematics['shoulder']['roll']}" scope="parent"/>
+    <xacro:property name="shoulder_pitch" value="${sec_kinematics['shoulder']['pitch']}" scope="parent"/>
+    <xacro:property name="shoulder_yaw" value="${sec_kinematics['shoulder']['yaw']}" scope="parent"/>
+
+    <xacro:property name="upper_arm_x" value="${sec_kinematics['upper_arm']['x']}" scope="parent"/>
+    <xacro:property name="upper_arm_y" value="${sec_kinematics['upper_arm']['y']}" scope="parent"/>
+    <xacro:property name="upper_arm_z" value="${sec_kinematics['upper_arm']['z']}" scope="parent"/>
+    <xacro:property name="upper_arm_roll" value="${sec_kinematics['upper_arm']['roll']}" scope="parent"/>
+    <xacro:property name="upper_arm_pitch" value="${sec_kinematics['upper_arm']['pitch']}" scope="parent"/>
+    <xacro:property name="upper_arm_yaw" value="${sec_kinematics['upper_arm']['yaw']}" scope="parent"/>
+
+    <xacro:property name="forearm_x" value="${sec_kinematics['forearm']['x']}" scope="parent"/>
+    <xacro:property name="forearm_y" value="${sec_kinematics['forearm']['y']}" scope="parent"/>
+    <xacro:property name="forearm_z" value="${sec_kinematics['forearm']['z']}" scope="parent"/>
+    <xacro:property name="forearm_roll" value="${sec_kinematics['forearm']['roll']}" scope="parent"/>
+    <xacro:property name="forearm_pitch" value="${sec_kinematics['forearm']['pitch']}" scope="parent"/>
+    <xacro:property name="forearm_yaw" value="${sec_kinematics['forearm']['yaw']}" scope="parent"/>
+
+    <xacro:property name="wrist_1_x" value="${sec_kinematics['wrist_1']['x']}" scope="parent"/>
+    <xacro:property name="wrist_1_y" value="${sec_kinematics['wrist_1']['y']}" scope="parent"/>
+    <xacro:property name="wrist_1_z" value="${sec_kinematics['wrist_1']['z']}" scope="parent"/>
+    <xacro:property name="wrist_1_roll" value="${sec_kinematics['wrist_1']['roll']}" scope="parent"/>
+    <xacro:property name="wrist_1_pitch" value="${sec_kinematics['wrist_1']['pitch']}" scope="parent"/>
+    <xacro:property name="wrist_1_yaw" value="${sec_kinematics['wrist_1']['yaw']}" scope="parent"/>
+
+    <xacro:property name="wrist_2_x" value="${sec_kinematics['wrist_2']['x']}" scope="parent"/>
+    <xacro:property name="wrist_2_y" value="${sec_kinematics['wrist_2']['y']}" scope="parent"/>
+    <xacro:property name="wrist_2_z" value="${sec_kinematics['wrist_2']['z']}" scope="parent"/>
+    <xacro:property name="wrist_2_roll" value="${sec_kinematics['wrist_2']['roll']}" scope="parent"/>
+    <xacro:property name="wrist_2_pitch" value="${sec_kinematics['wrist_2']['pitch']}" scope="parent"/>
+    <xacro:property name="wrist_2_yaw" value="${sec_kinematics['wrist_2']['yaw']}" scope="parent"/>
+
+    <xacro:property name="wrist_3_x" value="${sec_kinematics['wrist_3']['x']}" scope="parent"/>
+    <xacro:property name="wrist_3_y" value="${sec_kinematics['wrist_3']['y']}" scope="parent"/>
+    <xacro:property name="wrist_3_z" value="${sec_kinematics['wrist_3']['z']}" scope="parent"/>
+    <xacro:property name="wrist_3_roll" value="${sec_kinematics['wrist_3']['roll']}" scope="parent"/>
+    <xacro:property name="wrist_3_pitch" value="${sec_kinematics['wrist_3']['pitch']}" scope="parent"/>
+    <xacro:property name="wrist_3_yaw" value="${sec_kinematics['wrist_3']['yaw']}" scope="parent"/>
+
+    <!-- OFFSETS -->
+    <xacro:property name="shoulder_offset" value="${sec_offsets['shoulder_offset']}" scope="parent"/>
+    <xacro:property name="elbow_offset" value="${sec_offsets['elbow_offset']}" scope="parent"/>
+
+    <!-- INERTIA PARAMETERS -->
+    <!-- mass -->
+    <xacro:property name="base_mass" value="${sec_inertia_parameters['base_mass']}" scope="parent"/>
+    <xacro:property name="shoulder_mass" value="${sec_inertia_parameters['shoulder_mass']}" scope="parent"/>
+    <xacro:property name="upper_arm_mass" value="${sec_inertia_parameters['upper_arm_mass']}" scope="parent"/>
+    <xacro:property name="upper_arm_inertia_offset" value="${sec_inertia_parameters['upper_arm_inertia_offset']}" scope="parent"/>
+    <xacro:property name="forearm_mass" value="${sec_inertia_parameters['forearm_mass']}" scope="parent"/>
+    <xacro:property name="wrist_1_mass" value="${sec_inertia_parameters['wrist_1_mass']}" scope="parent"/>
+    <xacro:property name="wrist_2_mass" value="${sec_inertia_parameters['wrist_2_mass']}" scope="parent"/>
+    <xacro:property name="wrist_3_mass" value="${sec_inertia_parameters['wrist_3_mass']}" scope="parent"/>
+    <!-- link inertia parameter -->
+    <xacro:property name="intertia_links" value="${sec_inertia_parameters['links']}" scope="parent"/>
+    <xacro:property name="base_inertia_radius" value="${intertia_links['base']['radius']}" scope="parent"/>
+    <xacro:property name="base_inertia_length" value="${intertia_links['base']['length']}" scope="parent"/>
+    <xacro:property name="shoulder_inertia_radius" value="${intertia_links['shoulder']['radius']}" scope="parent"/>
+    <xacro:property name="shoulder_inertia_length" value="${intertia_links['shoulder']['length']}" scope="parent"/>
+    <xacro:property name="upperarm_inertia_radius" value="${intertia_links['upperarm']['radius']}" scope="parent"/>
+    <xacro:property name="upperarm_inertia_length" value="${intertia_links['upperarm']['length']}" scope="parent"/>
+    <xacro:property name="forearm_inertia_radius" value="${intertia_links['forearm']['radius']}" scope="parent"/>
+    <xacro:property name="forearm_inertia_length" value="${intertia_links['forearm']['length']}" scope="parent"/>
+    <xacro:property name="wrist_1_inertia_radius" value="${intertia_links['wrist_1']['radius']}" scope="parent"/>
+    <xacro:property name="wrist_1_inertia_length" value="${intertia_links['wrist_1']['length']}" scope="parent"/>
+    <xacro:property name="wrist_2_inertia_radius" value="${intertia_links['wrist_2']['radius']}" scope="parent"/>
+    <xacro:property name="wrist_2_inertia_length" value="${intertia_links['wrist_2']['length']}" scope="parent"/>
+    <xacro:property name="wrist_3_inertia_radius" value="${intertia_links['wrist_3']['radius']}" scope="parent"/>
+    <xacro:property name="wrist_3_inertia_length" value="${intertia_links['wrist_3']['length']}" scope="parent"/>
+
+    <!-- center of mass -->
+    <xacro:property name="prop_shoulder_cog" value="${sec_inertia_parameters['center_of_mass']['shoulder_cog']}" scope="parent"/>
+    <xacro:property name="prop_upper_arm_cog" value="${sec_inertia_parameters['center_of_mass']['upper_arm_cog']}" scope="parent"/>
+    <xacro:property name="prop_forearm_cog" value="${sec_inertia_parameters['center_of_mass']['forearm_cog']}" scope="parent"/>
+    <xacro:property name="prop_wrist_1_cog" value="${sec_inertia_parameters['center_of_mass']['wrist_1_cog']}" scope="parent"/>
+    <xacro:property name="prop__wrist_2_cog" value="${sec_inertia_parameters['center_of_mass']['wrist_2_cog']}" scope="parent"/>
+    <xacro:property name="prop_wrist_3_cog" value="${sec_inertia_parameters['center_of_mass']['wrist_3_cog']}" scope="parent"/>
+
+    <xacro:property name="shoulder_cog" value="${prop_shoulder_cog['x']} ${prop_shoulder_cog['y']} ${prop_shoulder_cog['z']}" scope="parent"/>
+    <xacro:property name="upper_arm_cog" value="${prop_upper_arm_cog['x']} ${prop_upper_arm_cog['y']} ${prop_upper_arm_cog['z']}" scope="parent"/>
+    <xacro:property name="forearm_cog" value="${prop_forearm_cog['x']} ${prop_forearm_cog['y']} ${prop_forearm_cog['z']}" scope="parent"/>
+    <xacro:property name="wrist_1_cog" value="${prop_wrist_1_cog['x']} ${prop_wrist_1_cog['y']} ${prop_wrist_1_cog['z']}" scope="parent"/>
+    <xacro:property name="wrist_2_cog" value="${prop__wrist_2_cog['x']} ${prop__wrist_2_cog['y']} ${prop__wrist_2_cog['z']}" scope="parent"/>
+    <xacro:property name="wrist_3_cog" value="${prop_wrist_3_cog['x']} ${prop_wrist_3_cog['y']} ${prop_wrist_3_cog['z']}" scope="parent"/>
+    <!-- cylinder radius -->
+    <xacro:property name="shoulder_radius" value="${sec_inertia_parameters['shoulder_radius']}" scope="parent"/>
+    <xacro:property name="upper_arm_radius" value="${sec_inertia_parameters['upper_arm_radius']}" scope="parent"/>
+    <xacro:property name="elbow_radius" value="${sec_inertia_parameters['elbow_radius']}" scope="parent"/>
+    <xacro:property name="forearm_radius" value="${sec_inertia_parameters['forearm_radius']}" scope="parent"/>
+    <xacro:property name="wrist_radius" value="${sec_inertia_parameters['wrist_radius']}" scope="parent"/>
+
+    <xacro:property
+        name="wrist_1_visual_offset"
+        value="${sec_mesh_files['wrist_1']['visual_offset']}"
+        scope="parent"/>
+
+    <xacro:property
+        name="wrist_2_visual_offset"
+        value="${sec_mesh_files['wrist_2']['visual_offset']}"
+        scope="parent"/>
+
+    <xacro:property
+        name="wrist_3_visual_offset"
+        value="${sec_mesh_files['wrist_3']['visual_offset']}"
+        scope="parent"/>
+
+    <!--KINEMATICS HASH-->
+    <xacro:property name="kinematics_hash" value="${sec_kinematics['hash']}" scope="parent"/>
+  </xacro:macro>
+</robot>
diff --git a/CustomRobots/robot_arms/models/ur3/inc/ur_transmissions.xacro b/CustomRobots/robot_arms/models/ur3/inc/ur_transmissions.xacro
new file mode 100644
index 000000000..18a86c355
--- /dev/null
+++ b/CustomRobots/robot_arms/models/ur3/inc/ur_transmissions.xacro
@@ -0,0 +1,73 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro">
+  <!--
+    NOTE the macro defined in this file is NOT part of the public API of this
+          package. Users CANNOT rely on this file being available, or stored in
+          this location. Nor can they rely on the existence of the macro.
+  -->
+
+  <xacro:macro name="ur_arm_transmission" params="prefix hw_interface">
+
+    <transmission name="${prefix}shoulder_pan_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}shoulder_pan_joint">
+        <hardwareInterface>${hw_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}shoulder_pan_motor">
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="${prefix}shoulder_lift_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}shoulder_lift_joint">
+        <hardwareInterface>${hw_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}shoulder_lift_motor">
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="${prefix}elbow_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}elbow_joint">
+        <hardwareInterface>${hw_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}elbow_motor">
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="${prefix}wrist_1_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}wrist_1_joint">
+        <hardwareInterface>${hw_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}wrist_1_motor">
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="${prefix}wrist_2_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}wrist_2_joint">
+        <hardwareInterface>${hw_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}wrist_2_motor">
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="${prefix}wrist_3_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${prefix}wrist_3_joint">
+        <hardwareInterface>${hw_interface}</hardwareInterface>
+      </joint>
+      <actuator name="${prefix}wrist_3_motor">
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+  </xacro:macro>
+
+</robot>
diff --git a/CustomRobots/robot_arms/models/ur3/ur.ros2_control.xacro b/CustomRobots/robot_arms/models/ur3/ur.ros2_control.xacro
new file mode 100644
index 000000000..1387428c5
--- /dev/null
+++ b/CustomRobots/robot_arms/models/ur3/ur.ros2_control.xacro
@@ -0,0 +1,304 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="ur_ros2_control" params="
+    name
+    prefix
+    use_fake_hardware:=false fake_sensor_commands:=false
+    sim_gazebo:=false
+    sim_gz:=false
+    headless_mode:=false
+    initial_positions:=${dict(shoulder_pan_joint=0.0,shoulder_lift_joint=-1.57,elbow_joint=0.0,wrist_1_joint=-1.57,wrist_2_joint=0.0,wrist_3_joint=0.0)}
+    use_tool_communication:=false
+    script_filename output_recipe_filename
+    input_recipe_filename tf_prefix
+    hash_kinematics robot_ip
+    tool_voltage:=24 tool_parity:=0 tool_baud_rate:=115200 tool_stop_bits:=1
+    tool_rx_idle_chars:=1.5 tool_tx_idle_chars:=3.5 tool_device_name:=/tmp/ttyUR tool_tcp_port:=54321
+    reverse_port:=50001
+    script_sender_port:=50002">
+
+    <ros2_control name="${name}" type="system">
+      <hardware>
+        <xacro:if value="${sim_gazebo}">
+          <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+        </xacro:if>
+        <xacro:if value="${sim_gz}">
+          <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+        </xacro:if>
+        <xacro:if value="${use_fake_hardware}">
+          <plugin>mock_components/GenericSystem</plugin>
+          <param name="fake_sensor_commands">${fake_sensor_commands}</param>
+          <param name="state_following_offset">0.0</param>
+        </xacro:if>
+        <xacro:unless value="${use_fake_hardware or sim_gazebo or sim_gz}">
+          <plugin>ur_robot_driver/URPositionHardwareInterface</plugin>
+          <param name="robot_ip">${robot_ip}</param>
+          <param name="script_filename">${script_filename}</param>
+          <param name="output_recipe_filename">${output_recipe_filename}</param>
+          <param name="input_recipe_filename">${input_recipe_filename}</param>
+          <param name="headless_mode">${headless_mode}</param>
+          <param name="reverse_port">${reverse_port}</param>
+          <param name="script_sender_port">${script_sender_port}</param>
+          <param name="tf_prefix">"${tf_prefix}"</param>
+          <param name="non_blocking_read">0</param>
+          <param name="servoj_gain">2000</param>
+          <param name="servoj_lookahead_time">0.03</param>
+          <param name="use_tool_communication">${use_tool_communication}</param>
+          <param name="kinematics/hash">${hash_kinematics}</param>
+          <param name="tool_voltage">${tool_voltage}</param>
+          <param name="tool_parity">${tool_parity}</param>
+          <param name="tool_baud_rate">${tool_baud_rate}</param>
+          <param name="tool_stop_bits">${tool_stop_bits}</param>
+          <param name="tool_rx_idle_chars">${tool_rx_idle_chars}</param>
+          <param name="tool_tx_idle_chars">${tool_tx_idle_chars}</param>
+          <param name="tool_device_name">${tool_device_name}</param>
+          <param name="tool_tcp_port">${tool_tcp_port}</param>
+        </xacro:unless>
+      </hardware>
+      <joint name="${prefix}shoulder_pan_joint">
+        <command_interface name="position">
+            <param name="min">{-2*pi}</param>
+            <param name="max">{2*pi}</param>
+        </command_interface>
+        <command_interface name="velocity">
+            <param name="min">-3.15</param>
+            <param name="max">3.15</param>
+        </command_interface>
+        <state_interface name="position">
+          <!-- initial position for the FakeSystem and simulation -->
+          <param name="initial_value">${initial_positions['shoulder_pan_joint']}</param>
+        </state_interface>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
+      <joint name="${prefix}shoulder_lift_joint">
+        <command_interface name="position">
+            <param name="min">{-2*pi}</param>
+            <param name="max">{2*pi}</param>
+        </command_interface>
+        <command_interface name="velocity">
+            <param name="min">-3.15</param>
+            <param name="max">3.15</param>
+        </command_interface>
+        <state_interface name="position">
+          <!-- initial position for the FakeSystem and simulation -->
+          <param name="initial_value">${initial_positions['shoulder_lift_joint']}</param>
+        </state_interface>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
+      <joint name="${prefix}elbow_joint">
+        <command_interface name="position">
+            <param name="min">{-pi}</param>
+            <param name="max">{pi}</param>
+        </command_interface>
+        <command_interface name="velocity">
+            <param name="min">-3.15</param>
+            <param name="max">3.15</param>
+        </command_interface>
+        <state_interface name="position">
+          <!-- initial position for the FakeSystem and simulation -->
+          <param name="initial_value">${initial_positions['elbow_joint']}</param>
+        </state_interface>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
+      <joint name="${prefix}wrist_1_joint">
+        <command_interface name="position">
+            <param name="min">{-2*pi}</param>
+            <param name="max">{2*pi}</param>
+        </command_interface>
+        <command_interface name="velocity">
+            <param name="min">-3.2</param>
+            <param name="max">3.2</param>
+        </command_interface>
+        <state_interface name="position">
+          <!-- initial position for the FakeSystem and simulation -->
+          <param name="initial_value">${initial_positions['wrist_1_joint']}</param>
+        </state_interface>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
+      <joint name="${prefix}wrist_2_joint">
+        <command_interface name="position">
+            <param name="min">{-2*pi}</param>
+            <param name="max">{2*pi}</param>
+        </command_interface>
+        <command_interface name="velocity">
+            <param name="min">-3.2</param>
+            <param name="max">3.2</param>
+        </command_interface>
+        <state_interface name="position">
+          <!-- initial position for the FakeSystem and simulation -->
+          <param name="initial_value">${initial_positions['wrist_2_joint']}</param>
+        </state_interface>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
+      <joint name="${prefix}wrist_3_joint">
+        <command_interface name="position">
+            <param name="min">{-2*pi}</param>
+            <param name="max">{2*pi}</param>
+        </command_interface>
+        <command_interface name="velocity">
+            <param name="min">-3.2</param>
+            <param name="max">3.2</param>
+        </command_interface>
+        <state_interface name="position">
+          <!-- initial position for the FakeSystem and simulation -->
+          <param name="initial_value">${initial_positions['wrist_3_joint']}</param>
+        </state_interface>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
+
+      <xacro:unless value="${sim_gazebo or sim_gz}">
+        <sensor name="tcp_fts_sensor">
+          <state_interface name="force.x"/>
+          <state_interface name="force.y"/>
+          <state_interface name="force.z"/>
+          <state_interface name="torque.x"/>
+          <state_interface name="torque.y"/>
+          <state_interface name="torque.z"/>
+        </sensor>
+
+        <!-- NOTE The following are joints used only for testing with fake hardware and will change in the future -->
+        <gpio name="speed_scaling">
+          <state_interface name="speed_scaling_factor"/>
+          <param name="initial_speed_scaling_factor">1</param>
+          <command_interface name="target_speed_fraction_cmd"/>
+          <command_interface name="target_speed_fraction_async_success"/>
+        </gpio>
+
+        <gpio name="gpio">
+          <command_interface name="standard_digital_output_cmd_0"/>
+          <command_interface name="standard_digital_output_cmd_1"/>
+          <command_interface name="standard_digital_output_cmd_2"/>
+          <command_interface name="standard_digital_output_cmd_3"/>
+          <command_interface name="standard_digital_output_cmd_4"/>
+          <command_interface name="standard_digital_output_cmd_5"/>
+          <command_interface name="standard_digital_output_cmd_6"/>
+          <command_interface name="standard_digital_output_cmd_7"/>
+          <command_interface name="standard_digital_output_cmd_8"/>
+          <command_interface name="standard_digital_output_cmd_9"/>
+          <command_interface name="standard_digital_output_cmd_10"/>
+          <command_interface name="standard_digital_output_cmd_11"/>
+          <command_interface name="standard_digital_output_cmd_12"/>
+          <command_interface name="standard_digital_output_cmd_13"/>
+          <command_interface name="standard_digital_output_cmd_14"/>
+          <command_interface name="standard_digital_output_cmd_15"/>
+          <command_interface name="standard_digital_output_cmd_16"/>
+          <command_interface name="standard_digital_output_cmd_17"/>
+
+          <command_interface name="standard_analog_output_cmd_0"/>
+          <command_interface name="standard_analog_output_cmd_1"/>
+
+          <command_interface name="io_async_success"/>
+
+          <state_interface name="digital_output_0"/>
+          <state_interface name="digital_output_1"/>
+          <state_interface name="digital_output_2"/>
+          <state_interface name="digital_output_3"/>
+          <state_interface name="digital_output_4"/>
+          <state_interface name="digital_output_5"/>
+          <state_interface name="digital_output_6"/>
+          <state_interface name="digital_output_7"/>
+          <state_interface name="digital_output_8"/>
+          <state_interface name="digital_output_9"/>
+          <state_interface name="digital_output_10"/>
+          <state_interface name="digital_output_11"/>
+          <state_interface name="digital_output_12"/>
+          <state_interface name="digital_output_13"/>
+          <state_interface name="digital_output_14"/>
+          <state_interface name="digital_output_15"/>
+          <state_interface name="digital_output_16"/>
+          <state_interface name="digital_output_17"/>
+
+          <state_interface name="digital_input_0"/>
+          <state_interface name="digital_input_1"/>
+          <state_interface name="digital_input_2"/>
+          <state_interface name="digital_input_3"/>
+          <state_interface name="digital_input_4"/>
+          <state_interface name="digital_input_5"/>
+          <state_interface name="digital_input_6"/>
+          <state_interface name="digital_input_7"/>
+          <state_interface name="digital_input_8"/>
+          <state_interface name="digital_input_9"/>
+          <state_interface name="digital_input_10"/>
+          <state_interface name="digital_input_11"/>
+          <state_interface name="digital_input_12"/>
+          <state_interface name="digital_input_13"/>
+          <state_interface name="digital_input_14"/>
+          <state_interface name="digital_input_15"/>
+          <state_interface name="digital_input_16"/>
+          <state_interface name="digital_input_17"/>
+
+          <state_interface name="standard_analog_output_0"/>
+          <state_interface name="standard_analog_output_1"/>
+
+          <state_interface name="standard_analog_input_0"/>
+          <state_interface name="standard_analog_input_1"/>
+
+          <state_interface name="analog_io_type_0"/>
+          <state_interface name="analog_io_type_1"/>
+          <state_interface name="analog_io_type_2"/>
+          <state_interface name="analog_io_type_3"/>
+
+          <state_interface name="tool_mode"/>
+          <state_interface name="tool_output_voltage"/>
+          <state_interface name="tool_output_current"/>
+          <state_interface name="tool_temperature"/>
+
+          <state_interface name="tool_analog_input_0"/>
+          <state_interface name="tool_analog_input_1"/>
+
+          <state_interface name="tool_analog_input_type_0"/>
+          <state_interface name="tool_analog_input_type_1"/>
+
+          <state_interface name="robot_mode"/>
+          <state_interface name="robot_status_bit_0"/>
+          <state_interface name="robot_status_bit_1"/>
+          <state_interface name="robot_status_bit_2"/>
+          <state_interface name="robot_status_bit_3"/>
+
+          <state_interface name="safety_mode"/>
+          <state_interface name="safety_status_bit_0"/>
+          <state_interface name="safety_status_bit_1"/>
+          <state_interface name="safety_status_bit_2"/>
+          <state_interface name="safety_status_bit_3"/>
+          <state_interface name="safety_status_bit_4"/>
+          <state_interface name="safety_status_bit_5"/>
+          <state_interface name="safety_status_bit_6"/>
+          <state_interface name="safety_status_bit_7"/>
+          <state_interface name="safety_status_bit_8"/>
+          <state_interface name="safety_status_bit_9"/>
+          <state_interface name="safety_status_bit_10"/>
+
+          <state_interface name="program_running"/>
+        </gpio>
+
+        <gpio name="payload">
+          <command_interface name="mass"/>
+          <command_interface name="cog.x"/>
+          <command_interface name="cog.y"/>
+          <command_interface name="cog.z"/>
+          <command_interface name="payload_async_success"/>
+        </gpio>
+
+        <gpio name="resend_robot_program">
+          <command_interface name="resend_robot_program_cmd"/>
+          <command_interface name="resend_robot_program_async_success"/>
+        </gpio>
+
+        <gpio name="system_interface">
+          <state_interface name="initialized"/>
+        </gpio>
+
+      </xacro:unless>
+
+    </ros2_control>
+
+  </xacro:macro>
+
+</robot>
\ No newline at end of file
diff --git a/CustomRobots/robot_arms/models/ur3/ur.urdf.xacro b/CustomRobots/robot_arms/models/ur3/ur.urdf.xacro
new file mode 100644
index 000000000..d940074dc
--- /dev/null
+++ b/CustomRobots/robot_arms/models/ur3/ur.urdf.xacro
@@ -0,0 +1,155 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro" name="$(arg name)">
+
+  <!-- robot name parameter -->
+  <xacro:arg name="name" default="ur"/>
+
+  <!-- import main macro -->
+  <xacro:include filename="$(find custom_robots)/models/ur3/ur_macro.xacro"/>
+
+  <!-- robot type -->
+  <xacro:arg name="ur_type" default="ur3"/>
+
+  <!-- parameters -->
+  <xacro:arg name="prefix" default="" />
+
+  <xacro:arg name="joint_limit_params"
+            default="$(find custom_robots)/config/ur3/joint_limits.yaml"/>
+
+  <xacro:arg name="kinematics_params"
+            default="$(find custom_robots)/config/ur3/default_kinematics.yaml"/>
+
+  <xacro:arg name="physical_params"
+            default="$(find custom_robots)/config/ur3/physical_parameters.yaml"/>
+
+  <xacro:arg name="visual_params"
+            default="$(find custom_robots)/config/ur3/visual_parameters.yaml"/>
+
+  <xacro:arg name="transmission_hw_interface" default=""/>
+
+  <xacro:arg name="safety_limits" default="false"/>
+  <xacro:arg name="safety_pos_margin" default="0.15"/>
+  <xacro:arg name="safety_k_position" default="20"/>
+
+  <!-- ros2_control related parameters -->
+  <xacro:arg name="headless_mode" default="false"/>
+
+  <xacro:arg name="robot_ip" default="0.0.0.0"/>
+
+  <xacro:arg name="script_filename" default=""/>
+  <xacro:arg name="output_recipe_filename" default=""/>
+  <xacro:arg name="input_recipe_filename" default=""/>
+
+  <!-- tool communication related parameters -->
+  <xacro:arg name="use_tool_communication" default="false"/>
+  <xacro:arg name="tool_voltage" default="24"/>
+  <xacro:arg name="tool_parity" default="0"/>
+  <xacro:arg name="tool_baud_rate" default="115200"/>
+  <xacro:arg name="tool_stop_bits" default="1"/>
+  <xacro:arg name="tool_rx_idle_chars" default="1.5"/>
+  <xacro:arg name="tool_tx_idle_chars" default="3.5"/>
+  <xacro:arg name="tool_device_name" default="/tmp/ttyUR"/>
+  <xacro:arg name="tool_tcp_port" default="54321"/>
+
+  <!-- Simulation parameters -->
+  <xacro:arg name="use_fake_hardware" default="false"/>
+  <xacro:arg name="fake_sensor_commands" default="false"/>
+
+  <xacro:arg name="sim_gazebo" default="false"/>
+  <xacro:arg name="sim_gz" default="true"/>
+
+  <xacro:arg name="simulation_controllers" default=""/>
+
+  <!-- initial position for simulations -->
+  <xacro:arg name="initial_positions_file"
+             default="$(find custom_robots)/config/initial_positions.yaml"/>
+
+  <!-- convert to property -->
+  <xacro:property name="initial_positions_file"
+                  default="$(arg initial_positions_file)"/>
+
+  <!-- create link fixed to world -->
+  <link name="world"/>
+
+  <!-- arm -->
+  <xacro:ur_robot
+      name="$(arg name)"
+      prefix="$(arg prefix)"
+      parent="world"
+
+      joint_limits_parameters_file="$(arg joint_limit_params)"
+      kinematics_parameters_file="$(arg kinematics_params)"
+      physical_parameters_file="$(arg physical_params)"
+      visual_parameters_file="$(arg visual_params)"
+
+      transmission_hw_interface="$(arg transmission_hw_interface)"
+
+      safety_limits="$(arg safety_limits)"
+      safety_pos_margin="$(arg safety_pos_margin)"
+      safety_k_position="$(arg safety_k_position)"
+
+      use_fake_hardware="$(arg use_fake_hardware)"
+      fake_sensor_commands="$(arg fake_sensor_commands)"
+
+      sim_gazebo="$(arg sim_gazebo)"
+      sim_gz="$(arg sim_gz)"
+
+      headless_mode="$(arg headless_mode)"
+
+      initial_positions="${load_yaml(initial_positions_file)}"
+
+      use_tool_communication="$(arg use_tool_communication)"
+      tool_voltage="$(arg tool_voltage)"
+      tool_parity="$(arg tool_parity)"
+      tool_baud_rate="$(arg tool_baud_rate)"
+      tool_stop_bits="$(arg tool_stop_bits)"
+      tool_rx_idle_chars="$(arg tool_rx_idle_chars)"
+      tool_tx_idle_chars="$(arg tool_tx_idle_chars)"
+      tool_device_name="$(arg tool_device_name)"
+      tool_tcp_port="$(arg tool_tcp_port)"
+
+      robot_ip="$(arg robot_ip)"
+
+      script_filename="$(arg script_filename)"
+      output_recipe_filename="$(arg output_recipe_filename)"
+      input_recipe_filename="$(arg input_recipe_filename)">
+
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+
+  </xacro:ur_robot>
+
+  <!-- Gazebo Classic -->
+  <xacro:if value="$(arg sim_gazebo)">
+    <gazebo reference="world">
+    </gazebo>
+
+    <gazebo>
+      <plugin filename="libgazebo_ros2_control.so"
+              name="gazebo_ros2_control">
+
+        <parameters>
+          $(arg simulation_controllers)
+        </parameters>
+
+      </plugin>
+    </gazebo>
+  </xacro:if>
+
+  <!-- Gazebo Harmonic -->
+  <xacro:if value="$(arg sim_gz)">
+    <gazebo reference="world">
+    </gazebo>
+
+    <gazebo>
+      <plugin filename="gz_ros2_control-system"
+              name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+
+        <parameters>
+          $(arg simulation_controllers)
+        </parameters>
+
+      </plugin>
+    </gazebo>
+  </xacro:if>
+
+</robot>
\ No newline at end of file
diff --git a/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro
new file mode 100644
index 000000000..630267ae4
--- /dev/null
+++ b/CustomRobots/robot_arms/models/ur3/ur3.urdf.xacro
@@ -0,0 +1,77 @@
+<?xml version="1.0" ?>
+<robot name="ur3" xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:arg name="name" default="ur"/>
+  <xacro:arg name="ur_type" default="ur3"/>
+  <xacro:arg name="prefix" default=""/>
+  <xacro:arg name="use_fake_hardware" default="false"/>
+  <xacro:arg name="fake_sensor_commands" default="false"/>
+  <xacro:arg name="sim_gazebo" default="false"/>
+  <xacro:arg name="sim_gz" default="true"/>
+  <xacro:arg name="simulation_controllers" default=""/>
+  <xacro:arg name="camera" default="false"/>
+  <xacro:arg name="robot_ip" default="0.0.0.0"/>
+  <xacro:arg name="script_filename" default=""/>
+  <xacro:arg name="input_recipe_filename" default=""/>
+  <xacro:arg name="output_recipe_filename" default=""/>
+
+  <!-- Include cams -->
+  <xacro:if value="$(arg camera)">
+
+    <xacro:include filename="$(find ros2srrc_ur5_gazebo)/urdf/common_properties.xacro"/>
+
+    <!-- HAND CAMERA -->
+    <xacro:include filename="$(find ros2srrc_ur5_gazebo)/urdf/cam_on_hand_params_gz.xacro"/>
+    <xacro:property name="parent_hand" value="tool0"/>
+    <xacro:include filename="$(find ros2srrc_ur5_gazebo)/urdf/cam_on_hand_gz.xacro"/>
+
+    <!-- BASE CAMERA -->
+    <xacro:include filename="$(find ros2srrc_ur5_gazebo)/urdf/cam_on_base_params_gz.xacro"/>
+    <xacro:property name="parent_base" value="base_link"/>
+    <!-- Posición -->
+    <xacro:property name="cam_base_px" value="0.5"/>
+    <xacro:property name="cam_base_py" value="0.0"/>
+    <xacro:property name="cam_base_pz" value="0.5"/>
+    <!-- Orientación -->
+    <xacro:property name="cam_base_or" value="0.0"/>
+    <xacro:property name="cam_base_op" value="1.57"/>
+    <xacro:property name="cam_base_oy" value="0.0"/>
+    <xacro:include filename="$(find ros2srrc_ur5_gazebo)/urdf/cam_on_base_gz.xacro"/>
+
+  </xacro:if>
+
+  <xacro:include filename="$(find custom_robots)/models/ur3/ur.urdf.xacro"/>
+  <xacro:include filename="$(find custom_robots)/models/ur3/ur.ros2_control.xacro"/>
+
+  <!-- Include XACRO-MACRO file of the Robotiq 2f-85: -->
+  <xacro:include filename="$(find ros2srrc_endeffectors)/robotiq_2f85/urdf/robotiq_2f85_macro.urdf.xacro"/>
+  <xacro:include filename="$(find ros2srrc_endeffectors)/robotiq_2f85/urdf/robotiq_2f85_ros2control.xacro"/>
+
+  <!-- Coupler -->
+  <link name="robotiq_coupler">
+    <visual>
+      <geometry>
+        <mesh filename="package://robotiq_description/meshes/robotiq_85_coupler.stl" />
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <cylinder radius="0.04" length="0.015"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="robotiq_coupler_joint" type="fixed">
+    <origin xyz="0 0 0.004" rpy="0 0 ${-pi/2.0}" />
+    <parent link="tool0"/>
+    <child link="robotiq_coupler"/>
+  </joint>
+
+  <!-- Gripper -->
+  <xacro:robotiq_85_gripper prefix="" parent="robotiq_coupler">
+    <origin xyz="0 0 0.004" rpy="0 ${-pi/2} ${pi}"/>
+  </xacro:robotiq_85_gripper>
+
+  <!-- ros2_control GRIPPER -->
+  <xacro:robotiq_85_ros2_control name="robotiq" prefix="" sim_gz="true" use_fake_hardware="false"/>
+
+</robot>
diff --git a/CustomRobots/robot_arms/models/ur3/ur_generated.urdf b/CustomRobots/robot_arms/models/ur3/ur_generated.urdf
new file mode 100644
index 000000000..e69de29bb
diff --git a/CustomRobots/robot_arms/models/ur3/ur_macro.xacro b/CustomRobots/robot_arms/models/ur3/ur_macro.xacro
new file mode 100644
index 000000000..011443f8b
--- /dev/null
+++ b/CustomRobots/robot_arms/models/ur3/ur_macro.xacro
@@ -0,0 +1,344 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  
+  <!-- Include UR3 ROBOT SPECIFICATIONS: -->
+  <xacro:include filename="$(find custom_robots)/models/ur3/inc/ur_transmissions.xacro"/>
+  <xacro:include filename="$(find custom_robots)/models/ur3/inc/ur_common.xacro"/>
+
+  <!-- XACRO-MACRO of UR3: -->
+  <xacro:macro name="ur_robot" params="
+    name
+    prefix
+    parent
+    *origin
+
+    joint_limits_parameters_file
+    kinematics_parameters_file
+    physical_parameters_file
+    visual_parameters_file
+
+    transmission_hw_interface:=hardware_interface/PositionJointInterface
+
+    safety_limits:=false
+    safety_pos_margin:=0.15
+    safety_k_position:=20
+
+    use_fake_hardware:=false
+    fake_sensor_commands:=false
+
+    sim_gazebo:=false
+    sim_gz:=false
+
+    headless_mode:=false
+
+    initial_positions:=${dict(
+        shoulder_pan_joint=0.0,
+        shoulder_lift_joint=-1.57,
+        elbow_joint=0.0,
+        wrist_1_joint=-1.57,
+        wrist_2_joint=0.0,
+        wrist_3_joint=0.0)}
+
+    use_tool_communication:=false
+
+    tool_voltage:=24
+    tool_parity:=0
+    tool_baud_rate:=115200
+    tool_stop_bits:=1
+    tool_rx_idle_chars:=1.5
+    tool_tx_idle_chars:=3.5
+
+    tool_device_name:=/tmp/ttyUR
+    tool_tcp_port:=54321
+
+    robot_ip:=0.0.0.0
+
+    script_filename:=to_be_filled_by_ur_robot_driver
+    output_recipe_filename:=to_be_filled_by_ur_robot_driver
+    input_recipe_filename:=to_be_filled_by_ur_robot_driver
+
+    reverse_port:=50001
+    script_sender_port:=50002">
+
+    <!-- LOAD UR3-Common: -->
+    <xacro:read_model_data
+        joint_limits_parameters_file="${joint_limits_parameters_file}"
+        kinematics_parameters_file="${kinematics_parameters_file}"
+        physical_parameters_file="${physical_parameters_file}"
+        visual_parameters_file="${visual_parameters_file}"
+        force_abs_paths="${sim_gazebo or sim_gz}"/>
+
+    <!-- ros2 control include -->
+    <xacro:include filename="$(find custom_robots)/models/ur3/ur.ros2_control.xacro" />
+
+    <!-- ros2 control instance -->
+    <xacro:ur_ros2_control
+    name="${name}"
+    prefix="${prefix}"
+    use_fake_hardware="${use_fake_hardware}"
+    initial_positions="${initial_positions}"
+    fake_sensor_commands="${fake_sensor_commands}"
+    headless_mode="${headless_mode}"
+    sim_gazebo="${sim_gazebo}"
+    sim_gz="${sim_gz}"
+    script_filename="${script_filename}"
+    output_recipe_filename="${output_recipe_filename}"
+    input_recipe_filename="${input_recipe_filename}"
+    tf_prefix=""
+    hash_kinematics="${kinematics_hash}"
+    robot_ip="${robot_ip}"
+    use_tool_communication="${use_tool_communication}"
+    tool_voltage="${tool_voltage}"
+    tool_parity="${tool_parity}"
+    tool_baud_rate="${tool_baud_rate}"
+    tool_stop_bits="${tool_stop_bits}"
+    tool_rx_idle_chars="${tool_rx_idle_chars}"
+    tool_tx_idle_chars="${tool_tx_idle_chars}"
+    tool_device_name="${tool_device_name}"
+    tool_tcp_port="${tool_tcp_port}"
+    reverse_port="${reverse_port}"
+    script_sender_port="${script_sender_port}"
+    />
+
+    <!-- ***** UR3 ROBOT .urdf SERIAL CHAIN ***** -->
+
+    <!-- Links: -->
+    <link name="${prefix}base_link"/>
+    <link name="${prefix}base_link_inertia">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
+        <geometry>
+          <mesh filename="file://$(find ros2srrc_robots)/ur3/meshes/visual/base.dae"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
+        <geometry>
+          <mesh filename="file://$(find ros2srrc_robots)/ur3/meshes/collision/base.stl"/>
+        </geometry>
+      </collision>
+      <xacro:cylinder_inertial radius="${base_inertia_radius}" length="${base_inertia_length}" mass="${base_mass}">
+        <origin xyz="0 0 0" rpy="0 0 0" />
+      </xacro:cylinder_inertial>
+    </link>
+    <link name="${prefix}shoulder_link">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
+        <geometry>
+          <mesh filename="file://$(find ros2srrc_robots)/ur3/meshes/visual/shoulder.dae"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
+        <geometry>
+          <mesh filename="file://$(find ros2srrc_robots)/ur3/meshes/collision/shoulder.stl"/>
+        </geometry>
+      </collision>
+      <xacro:cylinder_inertial radius="${shoulder_inertia_radius}" length="${shoulder_inertia_length}" mass="${shoulder_mass}">
+        <origin xyz="0 0 0" rpy="0 0 0" />
+      </xacro:cylinder_inertial>
+    </link>
+    <link name="${prefix}upper_arm_link">
+      <visual>
+        <origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
+        <geometry>
+          <mesh filename="file://$(find ros2srrc_robots)/ur3/meshes/visual/upperarm.dae"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
+        <geometry>
+          <mesh filename="file://$(find ros2srrc_robots)/ur3/meshes/collision/upperarm.stl"/>
+        </geometry>
+      </collision>
+      <xacro:cylinder_inertial radius="${upperarm_inertia_radius}" length="${upperarm_inertia_length}" mass="${upper_arm_mass}">
+        <origin xyz="${-0.5 * upperarm_inertia_length} 0.0 ${upper_arm_inertia_offset}" rpy="0 ${pi/2} 0" />
+      </xacro:cylinder_inertial>
+    </link>
+    <link name="${prefix}forearm_link">
+      <visual>
+        <origin xyz="0 0 ${elbow_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
+        <geometry>
+          <mesh filename="file://$(find ros2srrc_robots)/ur3/meshes/visual/forearm.dae"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 ${elbow_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
+        <geometry>
+          <mesh filename="file://$(find ros2srrc_robots)/ur3/meshes/collision/forearm.stl"/>
+        </geometry>
+      </collision>
+      <xacro:cylinder_inertial radius="${forearm_inertia_radius}" length="${forearm_inertia_length}"  mass="${forearm_mass}">
+        <origin xyz="${-0.5 * forearm_inertia_length} 0.0 ${elbow_offset}" rpy="0 ${pi/2} 0" />
+      </xacro:cylinder_inertial>
+    </link>
+    <link name="${prefix}wrist_1_link">
+      <visual>
+        <origin xyz="0 0 ${wrist_1_visual_offset}" rpy="${pi/2} 0 0"/>
+        <geometry>
+          <mesh filename="file://$(find ros2srrc_robots)/ur3/meshes/visual/wrist1.dae"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 ${wrist_1_visual_offset}" rpy="${pi/2} 0 0"/>
+        <geometry>
+          <mesh filename="file://$(find ros2srrc_robots)/ur3/meshes/collision/wrist1.stl"/>
+        </geometry>
+      </collision>
+      <xacro:cylinder_inertial radius="${wrist_1_inertia_radius}" length="${wrist_1_inertia_length}"  mass="${wrist_1_mass}">
+        <origin xyz="0 0 0" rpy="0 0 0" />
+      </xacro:cylinder_inertial>
+    </link>
+    <link name="${prefix}wrist_2_link">
+      <visual>
+        <origin xyz="0 0 ${wrist_2_visual_offset}" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="file://$(find ros2srrc_robots)/ur3/meshes/visual/wrist2.dae"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 ${wrist_2_visual_offset}" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="file://$(find ros2srrc_robots)/ur3/meshes/collision/wrist2.stl"/>
+        </geometry>
+      </collision>
+      <xacro:cylinder_inertial radius="${wrist_2_inertia_radius}" length="${wrist_2_inertia_length}"  mass="${wrist_2_mass}">
+        <origin xyz="0 0 0" rpy="0 0 0" />
+      </xacro:cylinder_inertial>
+    </link>
+    <link name="${prefix}wrist_3_link">
+      <visual>
+        <origin xyz="0 0 ${wrist_3_visual_offset}" rpy="${pi/2} 0 0"/>
+        <geometry>
+          <mesh filename="file://$(find ros2srrc_robots)/ur3/meshes/visual/wrist3.dae"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 ${wrist_3_visual_offset}" rpy="${pi/2} 0 0"/>
+        <geometry>
+          <mesh filename="file://$(find ros2srrc_robots)/ur3/meshes/collision/wrist3.stl"/>
+        </geometry>
+      </collision>
+      <xacro:cylinder_inertial radius="${wrist_3_inertia_radius}" length="${wrist_3_inertia_length}"  mass="${wrist_3_mass}">
+        <origin xyz="0.0 0.0 ${-0.5 * wrist_3_inertia_length}" rpy="0 0 0" />
+      </xacro:cylinder_inertial>
+    </link>
+
+    <!-- Joints: -->
+    <joint name="${prefix}base_joint" type="fixed">
+        <xacro:insert_block name="origin" />
+        <parent link="${parent}" />
+        <child link="${prefix}base_link" />
+    </joint>
+
+    <joint name="${prefix}base_link-base_link_inertia" type="fixed">
+      <parent link="${prefix}base_link" />
+      <child link="${prefix}base_link_inertia" />
+      <!-- 'base_link' is REP-103 aligned (so X+ forward), while the internal
+           frames of the robot/controller have X+ pointing backwards.
+           Use the joint between 'base_link' and 'base_link_inertia' (a dummy
+           link/frame) to introduce the necessary rotation over Z (of pi rad).
+      -->
+      <origin xyz="0 0 0" rpy="0 0 ${pi}" />
+    </joint>
+    <joint name="${prefix}shoulder_pan_joint" type="revolute">
+      <parent link="${prefix}base_link_inertia" />
+      <child link="${prefix}shoulder_link" />
+      <origin xyz="${shoulder_x} ${shoulder_y} ${shoulder_z}" rpy="${shoulder_roll} ${shoulder_pitch} ${shoulder_yaw}" />
+      <axis xyz="0 0 1" />
+      <limit lower="${shoulder_pan_lower_limit}" upper="${shoulder_pan_upper_limit}"
+        effort="${shoulder_pan_effort_limit}" velocity="${shoulder_pan_velocity_limit}"/>
+      <dynamics damping="1.0" friction="100.0"/>
+    </joint>
+    <joint name="${prefix}shoulder_lift_joint" type="revolute">
+      <parent link="${prefix}shoulder_link" />
+      <child link="${prefix}upper_arm_link" />
+      <origin xyz="${upper_arm_x} ${upper_arm_y} ${upper_arm_z}" rpy="${upper_arm_roll} ${upper_arm_pitch} ${upper_arm_yaw}" />
+      <axis xyz="0 0 1" />
+      <limit lower="${shoulder_lift_lower_limit}" upper="${shoulder_lift_upper_limit}"
+        effort="${shoulder_lift_effort_limit}" velocity="${shoulder_lift_velocity_limit}"/>
+      <dynamics damping="1.0" friction="100.0"/>
+    </joint>
+    <joint name="${prefix}elbow_joint" type="revolute">
+      <parent link="${prefix}upper_arm_link" />
+      <child link="${prefix}forearm_link" />
+      <origin xyz="${forearm_x} ${forearm_y} ${forearm_z}" rpy="${forearm_roll} ${forearm_pitch} ${forearm_yaw}" />
+      <axis xyz="0 0 1" />
+      <limit lower="${elbow_joint_lower_limit}" upper="${elbow_joint_upper_limit}"
+        effort="${elbow_joint_effort_limit}" velocity="${elbow_joint_velocity_limit}"/>
+      <dynamics damping="1.0" friction="100.0"/>
+    </joint>
+    <joint name="${prefix}wrist_1_joint" type="revolute">
+      <parent link="${prefix}forearm_link" />
+      <child link="${prefix}wrist_1_link" />
+      <origin xyz="${wrist_1_x} ${wrist_1_y} ${wrist_1_z}" rpy="${wrist_1_roll} ${wrist_1_pitch} ${wrist_1_yaw}" />
+      <axis xyz="0 0 1" />
+      <limit lower="${wrist_1_lower_limit}" upper="${wrist_1_upper_limit}"
+        effort="${wrist_1_effort_limit}" velocity="${wrist_1_velocity_limit}"/>
+      <dynamics damping="1.0" friction="10.0"/>
+    </joint>
+    <joint name="${prefix}wrist_2_joint" type="revolute">
+      <parent link="${prefix}wrist_1_link" />
+      <child link="${prefix}wrist_2_link" />
+      <origin xyz="${wrist_2_x} ${wrist_2_y} ${wrist_2_z}" rpy="${wrist_2_roll} ${wrist_2_pitch} ${wrist_2_yaw}" />
+      <axis xyz="0 0 1" />
+      <limit lower="${wrist_2_lower_limit}" upper="${wrist_2_upper_limit}"
+             effort="${wrist_2_effort_limit}" velocity="${wrist_2_velocity_limit}"/>
+      <dynamics damping="1.0" friction="10.0"/>
+    </joint>
+    <joint name="${prefix}wrist_3_joint" type="revolute">
+      <parent link="${prefix}wrist_2_link" />
+      <child link="${prefix}wrist_3_link" />
+      <origin xyz="${wrist_3_x} ${wrist_3_y} ${wrist_3_z}" rpy="${wrist_3_roll} ${wrist_3_pitch} ${wrist_3_yaw}" />
+      <axis xyz="0 0 1" />
+      <limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}"
+             effort="${wrist_3_effort_limit}" velocity="${wrist_3_velocity_limit}"/>
+      <dynamics damping="1.0" friction="1.0"/>
+    </joint>
+
+    <link name="${prefix}ft_frame"/>
+    <joint name="${prefix}wrist_3_link-ft_frame" type="fixed">
+        <parent link="${prefix}wrist_3_link"/>
+        <child link="${prefix}ft_frame"/>
+        <origin xyz="0 0 0" rpy="${pi} 0 0"/>
+    </joint>
+
+    <!-- ROS-Industrial 'base' frame - base_link to UR 'Base' Coordinates transform -->
+    <link name="${prefix}base"/>
+    <joint name="${prefix}base_link-base_fixed_joint" type="fixed">
+      <!-- Note the rotation over Z of pi radians - as base_link is REP-103
+           aligned (i.e., has X+ forward, Y+ left and Z+ up), this is needed
+           to correctly align 'base' with the 'Base' coordinate system of
+           the UR controller.
+      -->
+      <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
+      <parent link="${prefix}base_link"/>
+      <child link="${prefix}base"/>
+    </joint>
+
+    <!-- ROS-Industrial 'flange' frame - attachment point for EEF models -->
+    <link name="${prefix}flange" />
+    <joint name="${prefix}wrist_3-flange" type="fixed">
+      <parent link="${prefix}wrist_3_link" />
+      <child link="${prefix}flange" />
+      <origin xyz="0 0 0" rpy="0 ${-pi/2.0} ${-pi/2.0}" />
+    </joint>
+
+    <!-- ROS-Industrial 'tool0' frame - all-zeros tool frame -->
+    <link name="${prefix}tool0"/>
+    <joint name="${prefix}flange-tool0" type="fixed">
+      <!-- default toolframe - X+ left, Y+ up, Z+ front -->
+      <origin xyz="0 0 0" rpy="${pi/2.0} 0 ${pi/2.0}"/>
+      <parent link="${prefix}flange"/>
+      <child link="${prefix}tool0"/>
+    </joint>
+
+    <link name="${prefix}ee_link" />
+    <joint name="manipulator_dummy_joint" type="fixed">
+        <origin xyz="0 0 0.16" rpy="0 0 0" />
+        <parent link="${prefix}tool0"/>
+        <child link="${prefix}ee_link"/>
+    </joint>
+
+  </xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/CMakeLists.txt b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/CMakeLists.txt
new file mode 100644
index 000000000..b6bb475a3
--- /dev/null
+++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/CMakeLists.txt
@@ -0,0 +1,44 @@
+cmake_minimum_required(VERSION 3.5)
+project(ros2srrc_ur3_gazebo)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+install(
+  DIRECTORY 
+    config
+    urdf 
+    worlds
+  DESTINATION 
+    share/${PROJECT_NAME}
+)
+
+ament_package()
diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/config/configurations.yaml b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/config/configurations.yaml
new file mode 100644
index 000000000..f5ca050f7
--- /dev/null
+++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/config/configurations.yaml
@@ -0,0 +1,20 @@
+# ROS 2 Sim-to-Real Robot Control: UR3 Robot.
+Configurations:
+
+  - ID: "ur3_1"
+    Name: "UR3 on top of Robot Stand."
+    urdf: "ur3.urdf.xacro"
+    rob: "ur3"
+    ee: "none"
+
+  - ID: "ur3_2"
+    Name: "UR3 + Robotiq 2f-85 gripper on top of Robot Stand."
+    urdf: "ur3_robotiq_2f85.urdf.xacro"
+    rob: "ur3"
+    ee: "robotiq_2f85"
+
+  - ID: "ur3_3"
+    Name: "UR3 + Robotiq HandE gripper on top of Robot Stand."
+    urdf: "ur3_robotiq_hande.urdf.xacro"
+    rob: "ur3"
+    ee: "robotiq_hande"
\ No newline at end of file
diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/package.xml b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/package.xml
new file mode 100644
index 000000000..c3462821e
--- /dev/null
+++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/package.xml
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ros2srrc_ur3_gazebo</name>
+  <version>0.0.0</version>
+  <description>IFRA-Cranfield (Cranfield University, UK). Package: ROS 2 SimRealRobotControl - UR3-Gazebo</description>
+
+  <author>Mikel Bueno Viso</author>
+  <maintainer email="Mikel.Bueno-Viso@cranfield.ac.uk">Mikel Bueno Viso</maintainer>
+  
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3.urdf.xacro b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3.urdf.xacro
new file mode 100644
index 000000000..3916bc6f3
--- /dev/null
+++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3.urdf.xacro
@@ -0,0 +1,138 @@
+<?xml version="1.0" ?>
+
+<!-- 
+
+# ===================================== COPYRIGHT ===================================== #
+#                                                                                       #
+#  IFRA (Intelligent Flexible Robotics and Assembly) Group, CRANFIELD UNIVERSITY        #
+#  Created on behalf of the IFRA Group at Cranfield University, United Kingdom          #
+#  E-mail: IFRA@cranfield.ac.uk                                                         #
+#                                                                                       #
+#  Licensed under the Apache-2.0 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.                    #
+#                                                                                       #
+#  IFRA Group - Cranfield University                                                    #
+#  AUTHORS: Mikel Bueno Viso - Mikel.Bueno-Viso@cranfield.ac.uk                         #
+#           Dr. Seemal Asif  - s.asif@cranfield.ac.uk                                   #
+#           Prof. Phil Webb  - p.f.webb@cranfield.ac.uk                                 #
+#                                                                                       #
+#  Date: April, 2023.                                                                   #
+#                                                                                       #
+# ===================================== COPYRIGHT ===================================== #
+
+# ======= CITE OUR WORK ======= #
+# You can cite our work with the following statement:
+# IFRA-Cranfield (2023) ROS 2 Sim-to-Real Robot Control. URL: https://github.com/IFRA-Cranfield/ros2_SimRealRobotControl.
+
+-->
+
+<robot name="ur3" xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <!-- PASS ARGUMENTS: -->
+  <xacro:arg name="bringup" default="false"/>
+  <xacro:property name="bringup" value="$(arg bringup)"/>
+  <xacro:arg name="hmi" default="false"/>
+  <xacro:property name="hmi" value="$(arg hmi)"/>
+  <xacro:arg name="robot_ip" default="0.0.0.0"/>
+  <xacro:property name="robot_ip" value="$(arg robot_ip)"/>
+
+  <xacro:arg name="EE" default="false"/>
+  <xacro:property name="EE" value="$(arg EE)"/>
+  <xacro:arg name="EE_name" default="none"/>
+  <xacro:property name="EE_name" value="$(arg EE_name)"/>
+
+  <!-- UR3 Control ARGUMENTS -->
+  <xacro:arg name="script_filename" default="none"/>
+  <xacro:property name="script_filename" value="$(arg script_filename)"/>
+  <xacro:arg name="input_recipe_filename" default="none"/>
+  <xacro:property name="input_recipe_filename" value="$(arg input_recipe_filename)"/>
+  <xacro:arg name="output_recipe_filename" default="none"/>
+  <xacro:property name="output_recipe_filename" value="$(arg output_recipe_filename)"/>
+
+  <!-- Include XACRO-MACRO file of the UR3: -->
+  <xacro:include filename="$(find ros2srrc_robots)/ur3/urdf/ur3_macro.urdf.xacro"/>	
+  <xacro:ur3 
+    bringup="${bringup}"
+    robot_ip="${robot_ip}"
+    EE="${EE}"
+    EE_name="${EE_name}"
+
+    script_filename="${script_filename}"
+    input_recipe_filename="${input_recipe_filename}"
+    output_recipe_filename="${output_recipe_filename}"
+  />
+
+  <!-- Load ROS 2 Gazebo Plugin -->
+  <xacro:unless value="${bringup}">
+    
+    <gazebo>
+      <plugin filename="gz_ros2_control-system"
+              name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+
+        <parameters>
+          $(find ros2srrc_robots)/ur3/config/controller.yaml
+        </parameters>
+
+      </plugin>
+    </gazebo>
+
+  </xacro:unless>
+
+  <!-- World: LINK -->
+  <link name="world"/>
+  
+  <!-- Robot Stand: LINK -->
+  <link name='robot_stand'>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="100"/>
+      <inertia ixx="0.01"  ixy="0"  ixz="0" iyy="0.01" iyz="0" izz="0.01" />
+    </inertial>
+    <collision name='collision'>
+      <origin xyz="0 0 0" rpy=" 0 0 0"/> 
+      <geometry>
+        <box size = "0.2 0.2 1.0"/>
+      </geometry>
+    </collision>
+    <visual name='chassis_visual'>
+      <origin xyz="0 0 0" rpy=" 0 0 0"/>
+      <geometry>
+        <box size = "0.2 0.2 1.0"/>
+      </geometry>
+    </visual>
+  </link>
+
+  <!-- Robot Stand: FIXED JOINT -->
+  <joint name="world_joint" type="fixed">
+    <parent link= "world" />
+    <child link = "robot_stand" />
+    <origin xyz="0.0 0.0 0.5" rpy="0.0 0.0 0.0" />
+  </joint>
+
+  <!-- Robot Stand: Robot+Stand JOINT -->
+  <joint name="stand_joint" type="fixed">
+    <parent link= "robot_stand" />
+    <child link = "base_link" />
+    <origin xyz="0.0 0.0 0.505" rpy="0.0 0.0 0.0" /> 
+  </joint>
+  
+  <!-- ros2srrc_HMI CAMERA -->
+  <xacro:if value="${hmi}">
+    <xacro:include filename="$(find ros2srrc_hmi)/urdf/hmicamera.urdf.xacro"/>	
+    <xacro:HMI_camera 
+      x="2.0"
+      y="1.25"
+      z="1.5"
+      roll="0.0"
+      pitch="0.35"
+      yaw="-2.6"
+    />
+  </xacro:if>
+
+</robot>
diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro
new file mode 100644
index 000000000..c65c22c45
--- /dev/null
+++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_2f85.urdf.xacro
@@ -0,0 +1,140 @@
+<?xml version="1.0" ?>
+
+<!-- 
+
+# ===================================== COPYRIGHT ===================================== #
+#                                                                                       #
+#  IFRA (Intelligent Flexible Robotics and Assembly) Group, CRANFIELD UNIVERSITY        #
+#  Created on behalf of the IFRA Group at Cranfield University, United Kingdom          #
+#  E-mail: IFRA@cranfield.ac.uk                                                         #
+#                                                                                       #
+#  Licensed under the Apache-2.0 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.                    #
+#                                                                                       #
+#  IFRA Group - Cranfield University                                                    #
+#  AUTHORS: Mikel Bueno Viso - Mikel.Bueno-Viso@cranfield.ac.uk                         #
+#           Dr. Seemal Asif  - s.asif@cranfield.ac.uk                                   #
+#           Prof. Phil Webb  - p.f.webb@cranfield.ac.uk                                 #
+#                                                                                       #
+#  Date: April, 2023.                                                                   #
+#                                                                                       #
+# ===================================== COPYRIGHT ===================================== #
+
+# ======= CITE OUR WORK ======= #
+# You can cite our work with the following statement:
+# IFRA-Cranfield (2023) ROS 2 Sim-to-Real Robot Control. URL: https://github.com/IFRA-Cranfield/ros2_SimRealRobotControl.
+
+-->
+
+<robot name="ur3" xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <!-- PASS ARGUMENTS: -->
+  <xacro:arg name="bringup" default="false"/>
+  <xacro:property name="bringup" value="$(arg bringup)"/>
+  <xacro:arg name="hmi" default="false"/>
+  <xacro:property name="hmi" value="$(arg hmi)"/>
+  <xacro:arg name="robot_ip" default="0.0.0.0"/>
+  <xacro:property name="robot_ip" value="$(arg robot_ip)"/>
+
+  <xacro:arg name="EE" default="false"/>
+  <xacro:property name="EE" value="$(arg EE)"/>
+  <xacro:arg name="EE_name" default="none"/>
+  <xacro:property name="EE_name" value="$(arg EE_name)"/>
+  <xacro:arg name="camera" default="false"/>
+  <xacro:property name="camera" value="$(arg camera)"/>
+
+  <!-- UR3 Control ARGUMENTS -->
+  <xacro:arg name="script_filename" default="none"/>
+  <xacro:property name="script_filename" value="$(arg script_filename)"/>
+  <xacro:arg name="input_recipe_filename" default="none"/>
+  <xacro:property name="input_recipe_filename" value="$(arg input_recipe_filename)"/>
+  <xacro:arg name="output_recipe_filename" default="none"/>
+  <xacro:property name="output_recipe_filename" value="$(arg output_recipe_filename)"/>
+
+  <!-- Include cams -->
+  <xacro:if value="${camera}">
+
+    <xacro:include filename="$(find ros2srrc_ur5_gazebo)/urdf/common_properties.xacro"/>
+
+    <!-- HAND CAMERA -->
+    <xacro:include filename="$(find ros2srrc_ur5_gazebo)/urdf/cam_on_hand_params_gz.xacro"/>
+    <xacro:property name="parent_hand" value="tool0"/>
+    <xacro:include filename="$(find ros2srrc_ur5_gazebo)/urdf/cam_on_hand_gz.xacro"/>
+
+    <!-- BASE CAMERA -->
+    <xacro:include filename="$(find ros2srrc_ur5_gazebo)/urdf/cam_on_base_params_gz.xacro"/>
+    <xacro:property name="parent_base" value="base_link"/>
+    <xacro:property name="cam_base_pz" value="0.5"/>
+    <xacro:include filename="$(find ros2srrc_ur5_gazebo)/urdf/cam_on_base_gz.xacro"/>
+
+  </xacro:if>
+
+  <!-- Include XACRO-MACRO file of the UR3: -->
+  <xacro:include filename="$(find ros2srrc_robots)/ur3/urdf/ur3_macro.urdf.xacro"/>
+  <xacro:ur3
+    bringup="${bringup}"
+    robot_ip="${robot_ip}"
+    EE="${EE}"
+    EE_name="${EE_name}"
+    script_filename="${script_filename}"
+    input_recipe_filename="${input_recipe_filename}"
+    output_recipe_filename="${output_recipe_filename}"
+  />
+
+  <!-- Include XACRO-MACRO file of the Robotiq 2f-85: -->
+  <xacro:include filename="$(find ros2srrc_endeffectors)/robotiq_2f85/urdf/robotiq_2f85_macro.urdf.xacro"/>	
+  <xacro:robotiq_2f85 
+    bringup="${bringup}"
+    parent_link="tool0"
+  />
+
+
+  <!-- Load ROS 2 Gazebo Plugin -->
+  <xacro:unless value="${bringup}">
+    
+    <gazebo>
+      <plugin filename="gz_ros2_control-system"
+              name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+
+        <parameters>
+          $(find ros2srrc_robots)/ur3/config/controller.yaml
+        </parameters>
+
+        <parameters>
+          $(find ros2srrc_endeffectors)/${EE_name}/config/controller.yaml
+        </parameters>
+
+      </plugin>
+    </gazebo>
+
+  </xacro:unless> 
+
+  <!-- ros2srrc_HMI CAMERA -->
+  <xacro:if value="${hmi}">
+    <xacro:include filename="$(find ros2srrc_hmi)/urdf/hmicamera.urdf.xacro"/>	
+    <xacro:HMI_camera 
+      x="2.0"
+      y="1.25"
+      z="1.5"
+      roll="0.0"
+      pitch="0.35"
+      yaw="-2.6"
+    />
+  </xacro:if>
+
+  <!-- Fijar el robot al mundo -->
+  <link name="world"/>
+
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="base_link"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+
+</robot>
diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_hande.urdf.xacro b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_hande.urdf.xacro
new file mode 100644
index 000000000..50d19205e
--- /dev/null
+++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/urdf/ur3_robotiq_hande.urdf.xacro
@@ -0,0 +1,149 @@
+<?xml version="1.0" ?>
+
+<!-- 
+
+# ===================================== COPYRIGHT ===================================== #
+#                                                                                       #
+#  IFRA (Intelligent Flexible Robotics and Assembly) Group, CRANFIELD UNIVERSITY        #
+#  Created on behalf of the IFRA Group at Cranfield University, United Kingdom          #
+#  E-mail: IFRA@cranfield.ac.uk                                                         #
+#                                                                                       #
+#  Licensed under the Apache-2.0 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.                    #
+#                                                                                       #
+#  IFRA Group - Cranfield University                                                    #
+#  AUTHORS: Mikel Bueno Viso - Mikel.Bueno-Viso@cranfield.ac.uk                         #
+#           Dr. Seemal Asif  - s.asif@cranfield.ac.uk                                   #
+#           Prof. Phil Webb  - p.f.webb@cranfield.ac.uk                                 #
+#                                                                                       #
+#  Date: April, 2023.                                                                   #
+#                                                                                       #
+# ===================================== COPYRIGHT ===================================== #
+
+# ======= CITE OUR WORK ======= #
+# You can cite our work with the following statement:
+# IFRA-Cranfield (2023) ROS 2 Sim-to-Real Robot Control. URL: https://github.com/IFRA-Cranfield/ros2_SimRealRobotControl.
+
+-->
+
+<robot name="ur3" xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <!-- PASS ARGUMENTS: -->
+  <xacro:arg name="bringup" default="false"/>
+  <xacro:property name="bringup" value="$(arg bringup)"/>
+  <xacro:arg name="hmi" default="false"/>
+  <xacro:property name="hmi" value="$(arg hmi)"/>
+  <xacro:arg name="robot_ip" default="0.0.0.0"/>
+  <xacro:property name="robot_ip" value="$(arg robot_ip)"/>
+
+  <xacro:arg name="EE" default="false"/>
+  <xacro:property name="EE" value="$(arg EE)"/>
+  <xacro:arg name="EE_name" default="none"/>
+  <xacro:property name="EE_name" value="$(arg EE_name)"/>
+
+  <!-- UR3 Control ARGUMENTS -->
+  <xacro:arg name="script_filename" default="none"/>
+  <xacro:property name="script_filename" value="$(arg script_filename)"/>
+  <xacro:arg name="input_recipe_filename" default="none"/>
+  <xacro:property name="input_recipe_filename" value="$(arg input_recipe_filename)"/>
+  <xacro:arg name="output_recipe_filename" default="none"/>
+  <xacro:property name="output_recipe_filename" value="$(arg output_recipe_filename)"/>
+
+  <!-- Include XACRO-MACRO file of the UR3: -->
+  <xacro:include filename="$(find ros2srrc_robots)/ur3/urdf/ur3_macro.urdf.xacro"/>	
+  <xacro:ur3 
+    bringup="${bringup}"
+    robot_ip="${robot_ip}"
+    EE="${EE}"
+    EE_name="${EE_name}"
+
+    script_filename="${script_filename}"
+    input_recipe_filename="${input_recipe_filename}"
+    output_recipe_filename="${output_recipe_filename}"
+  />
+
+  <!-- Include XACRO-MACRO file of the Robotiq 2f-85: -->
+  <xacro:include filename="$(find ros2srrc_endeffectors)/robotiq_hande/urdf/robotiq_hande_macro.urdf.xacro"/>	
+  <xacro:robotiq_hande
+    bringup="${bringup}"
+    parent_link="tool0"
+  />
+
+  <!-- Load ROS 2 Gazebo Plugin -->
+  <xacro:unless value="${bringup}">
+    
+    <gazebo>
+      <plugin filename="gz_ros2_control-system"
+              name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+
+        <parameters>
+          $(find ros2srrc_robots)/ur3/config/controller.yaml
+        </parameters>
+
+        <parameters>
+          $(find ros2srrc_endeffectors)/${EE_name}/config/controller.yaml
+        </parameters>
+
+      </plugin>
+    </gazebo>
+
+  </xacro:unless>
+
+  <!-- World: LINK -->
+  <link name="world"/>
+  
+  <!-- Robot Stand: LINK -->
+  <link name='robot_stand'>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="100"/>
+      <inertia ixx="0.01"  ixy="0"  ixz="0" iyy="0.01" iyz="0" izz="0.01" />
+    </inertial>
+    <collision name='collision'>
+      <origin xyz="0 0 0" rpy=" 0 0 0"/> 
+      <geometry>
+        <box size = "0.2 0.2 0.5"/>
+      </geometry>
+    </collision>
+    <visual name='chassis_visual'>
+      <origin xyz="0 0 0" rpy=" 0 0 0"/>
+      <geometry>
+        <box size = "0.2 0.2 0.5"/>
+      </geometry>
+    </visual>
+  </link>
+
+  <!-- Robot Stand: FIXED JOINT -->
+  <joint name="world_joint" type="fixed">
+    <parent link= "world" />
+    <child link = "robot_stand" />
+    <origin xyz="0.0 0.0 0.25" rpy="0.0 0.0 0.0" />
+  </joint>
+
+  <!-- Robot Stand: Robot+Stand JOINT -->
+  <joint name="stand_joint" type="fixed">
+    <parent link= "robot_stand" />
+    <child link = "base_link" />
+    <origin xyz="0.0 0.0 0.255" rpy="0.0 0.0 0.0" /> 
+  </joint>
+
+  <!-- ros2srrc_HMI CAMERA -->
+  <xacro:if value="${hmi}">
+    <xacro:include filename="$(find ros2srrc_hmi)/urdf/hmicamera.urdf.xacro"/>	
+    <xacro:HMI_camera 
+      x="2.0"
+      y="1.25"
+      z="1.5"
+      roll="0.0"
+      pitch="0.35"
+      yaw="-2.6"
+    />
+  </xacro:if>
+
+</robot>
\ No newline at end of file
diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/worlds/ros2srrc_ur3.world b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/worlds/ros2srrc_ur3.world
new file mode 100644
index 000000000..fa1e98d6d
--- /dev/null
+++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/worlds/ros2srrc_ur3.world
@@ -0,0 +1,1611 @@
+<?xml version="1.0" ?>
+<sdf version='1.6'>
+  <world name='default'>
+    <light name='sun' type='directional'>
+      <cast_shadows>1</cast_shadows>
+      <pose frame=''>0 0 10 0 -0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+    <model name='ground_plane'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <surface>
+            <contact>
+              <collide_bitmask>65535</collide_bitmask>
+              <ode/>
+            </contact>
+            <friction>
+              <ode>
+                <mu>100</mu>
+                <mu2>50</mu2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+    </model>
+    <gravity>0 0 -9.8</gravity>
+    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
+    <atmosphere type='adiabatic'/>
+    <physics name='default_physics' default='0' type='ode'>
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <real_time_update_rate>1000</real_time_update_rate>
+    </physics>
+    <scene>
+      <ambient>0.4 0.4 0.4 1</ambient>
+      <background>0.7 0.7 0.7 1</background>
+      <shadows>1</shadows>
+    </scene>
+    <wind/>
+    <spherical_coordinates>
+      <surface_model>EARTH_WGS84</surface_model>
+      <latitude_deg>0</latitude_deg>
+      <longitude_deg>0</longitude_deg>
+      <elevation>0</elevation>
+      <heading_deg>0</heading_deg>
+    </spherical_coordinates>
+    <model name='conveyor'>
+      <link name='link'>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <gravity>1</gravity>
+        <inertial>
+          <mass>1</mass>
+          <pose frame=''>0 0 0 0 -0 0</pose>
+          <inertia>
+            <ixx>1</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1</iyy>
+            <iyz>0</iyz>
+            <izz>1</izz>
+          </inertia>
+        </inertial>
+        <visual name='visual'>
+          <pose frame=''>0 0 0.95 0 -0 0</pose>
+          <geometry>
+            <mesh>
+              <scale>0.45 0.35 0.5</scale>
+              <uri>model://conveyor/meshes/conveyor.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name='collision_0'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>0 0 0.95 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>2.4 0.8 0.1</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+      </link>
+      <static>1</static>
+      <allow_auto_disable>1</allow_auto_disable>
+      <pose frame=''>0.335211 0.377742 0 0 -0 0</pose>
+    </model>
+    <model name='ground_logo_big'>
+      <static>1</static>
+      <link name='link'>
+        <visual name='visual'>
+          <pose frame=''>0 0 0.001 0 -0 0</pose>
+          <geometry>
+            <mesh>
+              <scale>1 1 1</scale>
+              <uri>model://ground_logo_big/meshes/ground_logo.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose frame=''>-0.163565 0.817691 0 0 -0 0</pose>
+    </model>
+    <state world_name='default'>
+      <sim_time>1275 417000000</sim_time>
+      <real_time>538 186819528</real_time>
+      <wall_time>1594223523 209571631</wall_time>
+      <iterations>534891</iterations>
+      <model name='blue_target'>
+        <pose frame=''>-0.6 0.45 0.755 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose frame=''>-0.6 0.45 0.755 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='conveyor'>
+        <pose frame=''>0.6 0 0 0 -0 1.57</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose frame=''>0.6 0 0 0 -0 1.57</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='ground_logo_big'>
+        <pose frame=''>-0.5 0 0 0 -0 1.57</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose frame=''>-0.5 0 0 0 -0 1.57</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='ground_plane'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose frame=''>0 0 0 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='red_target'>
+        <pose frame=''>-0.6 0.15 0.755 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose frame=''>-0.6 0.15 0.755 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='target_table'>
+        <pose frame=''>-0.6 0 0 0 -0 1.57</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose frame=''>-0.6 0 0 0 -0 1.57</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='yellow_target'>
+        <pose frame=''>-0.6 -0.45 0.755 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose frame=''>-0.6 -0.45 0.755 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='green_target'>
+        <pose frame=''>-0.6 -0.15 0.755 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose frame=''>-0.6 -0.15 0.755 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <light name='sun'>
+        <pose frame=''>0 0 10 0 -0 0</pose>
+      </light>
+    </state>
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose frame=''>3.82249 4.74978 2.99885 -0 0.401795 -2.20437</pose>
+        <view_controller>orbit</view_controller>
+        <projection_type>perspective</projection_type>
+      </camera>
+    </gui>
+    <model name='target_table'>
+      <link name='link'>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <gravity>1</gravity>
+        <inertial>
+          <mass>1</mass>
+          <pose frame=''>0 0 0 0 -0 0</pose>
+          <inertia>
+            <ixx>1</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1</iyy>
+            <iyz>0</iyz>
+            <izz>1</izz>
+          </inertia>
+        </inertial>
+        <visual name='visual'>
+          <pose frame=''>0 0 0.73 0 -0 1.57</pose>
+          <geometry>
+            <mesh>
+              <scale>1 1 1</scale>
+              <uri>model://target_table/meshes/target_table.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name='collision_0'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>0 0 0.73 0 -0 1.57</pose>
+          <geometry>
+            <box>
+              <size>0.8 1.6 0.06</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+      </link>
+      <static>1</static>
+      <allow_auto_disable>1</allow_auto_disable>
+      <pose frame=''>-0.877342 -1.1194 0 0 -0 0</pose>
+    </model>
+    <model name='yellow_target'>
+      <static>1</static>
+      <link name='link'>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <gravity>1</gravity>
+        <inertial>
+          <mass>1</mass>
+          <pose frame=''>0 0 0 0 -0 0</pose>
+          <inertia>
+            <ixx>1</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1</iyy>
+            <iyz>0</iyz>
+            <izz>1</izz>
+          </inertia>
+        </inertial>
+        <visual name='visual'>
+          <pose frame=''>0 0 0.01 0 -0 0</pose>
+          <geometry>
+            <mesh>
+              <scale>0.5 0.5 0.5</scale>
+              <uri>model://yellow_target/meshes/yellow_target.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name='front_wall'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>0.144 0 0 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.012 0.3 0.24</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+        <collision name='right_wall'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>0 -0.144 0 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.3 0.012 0.36</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+        <collision name='bottom'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>0 0 0 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.3 0.3 0.025</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+        <collision name='left_wall'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>0 0.144 0 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.3 0.012 0.39</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+        <collision name='back_wall'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>-0.144 0 0 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.012 0.3 0.39</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+      </link>
+      <pose frame=''>-1.65226 -0.48866 0 0 -0 0</pose>
+    </model>
+    <model name='green_target'>
+      <static>1</static>
+      <link name='link'>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <gravity>1</gravity>
+        <inertial>
+          <mass>1</mass>
+          <pose frame=''>0 0 0 0 -0 0</pose>
+          <inertia>
+            <ixx>1</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1</iyy>
+            <iyz>0</iyz>
+            <izz>1</izz>
+          </inertia>
+        </inertial>
+        <visual name='visual'>
+          <pose frame=''>0 0 0.01 0 -0 0</pose>
+          <geometry>
+            <mesh>
+              <scale>0.5 0.5 0.5</scale>
+              <uri>model://green_target/meshes/green_target.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name='front_wall'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>0.144 0 0 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.012 0.3 0.24</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+        <collision name='right_wall'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>0 -0.144 0 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.3 0.012 0.36</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+        <collision name='bottom'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>0 0 0 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.3 0.3 0.025</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+        <collision name='left_wall'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>0 0.144 0 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.3 0.012 0.39</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+        <collision name='back_wall'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>-0.144 0 0 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.012 0.3 0.39</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+      </link>
+      <pose frame=''>-1.65226 -0.48866 0 0 -0 0</pose>
+    </model>
+    <model name='red_target'>
+      <static>1</static>
+      <link name='link'>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <gravity>1</gravity>
+        <inertial>
+          <mass>1</mass>
+          <pose frame=''>0 0 0 0 -0 0</pose>
+          <inertia>
+            <ixx>1</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1</iyy>
+            <iyz>0</iyz>
+            <izz>1</izz>
+          </inertia>
+        </inertial>
+        <visual name='visual'>
+          <pose frame=''>0 0 0.01 0 -0 0</pose>
+          <geometry>
+            <mesh>
+              <scale>0.5 0.5 0.5</scale>
+              <uri>model://red_target/meshes/red_target.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name='front_wall'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>0.144 0 0 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.012 0.3 0.24</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+        <collision name='right_wall'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>0 -0.144 0 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.3 0.012 0.36</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+        <collision name='bottom'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>0 0 0 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.3 0.3 0.025</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+        <collision name='left_wall'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>0 0.144 0 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.3 0.012 0.39</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+        <collision name='back_wall'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>-0.144 0 0 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.012 0.3 0.39</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+      </link>
+      <pose frame=''>-0.055993 0.310239 0 0 -0 0</pose>
+    </model>
+    <model name='blue_target'>
+      <static>1</static>
+      <link name='link'>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <gravity>1</gravity>
+        <inertial>
+          <mass>1</mass>
+          <pose frame=''>0 0 0 0 -0 0</pose>
+          <inertia>
+            <ixx>1</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1</iyy>
+            <iyz>0</iyz>
+            <izz>1</izz>
+          </inertia>
+        </inertial>
+        <visual name='visual'>
+          <pose frame=''>0 0 0.01 0 -0 0</pose>
+          <geometry>
+            <mesh>
+              <scale>0.5 0.5 0.5</scale>
+              <uri>model://blue_target/meshes/blue_target.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name='front_wall'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>0.144 0 0 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.012 0.3 0.24</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+        <collision name='right_wall'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>0 -0.144 0 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.3 0.012 0.36</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+        <collision name='bottom'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>0 0 0 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.3 0.3 0.025</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+        <collision name='left_wall'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>0 0.144 0 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.3 0.012 0.39</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+        <collision name='back_wall'>
+          <laser_retro>0</laser_retro>
+          <max_contacts>10</max_contacts>
+          <pose frame=''>-0.144 0 0 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.012 0.3 0.39</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <coefficient>1</coefficient>
+                <patch_radius>0</patch_radius>
+                <surface_radius>0</surface_radius>
+                <use_patch_radius>1</use_patch_radius>
+                <ode>
+                  <slip>0</slip>
+                </ode>
+              </torsional>
+            </friction>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1e+06</threshold>
+            </bounce>
+            <contact>
+              <collide_without_contact>0</collide_without_contact>
+              <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+              <collide_bitmask>1</collide_bitmask>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+              <bullet>
+                <split_impulse>1</split_impulse>
+                <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e+13</kp>
+                <kd>1</kd>
+              </bullet>
+            </contact>
+          </surface>
+        </collision>
+      </link>
+      <pose frame=''>-1.18752 -1.7319 0 0 -0 0</pose>
+    </model>
+
+
+
+    <!-- Link Attacher: GAZEBO PLUGIN -->
+    <plugin name="gazebo_link_attacher" filename="libgazebo_link_attacher.so"/>
+
+  </world>
+</sdf>
diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/worlds/ros2srrc_ur3_original.world b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/worlds/ros2srrc_ur3_original.world
new file mode 100644
index 000000000..00314e1d9
--- /dev/null
+++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_gazebo/worlds/ros2srrc_ur3_original.world
@@ -0,0 +1,59 @@
+<?xml version="1.0" ?>
+
+<!-- 
+
+# ===================================== COPYRIGHT ===================================== #
+#                                                                                       #
+#  IFRA (Intelligent Flexible Robotics and Assembly) Group, CRANFIELD UNIVERSITY        #
+#  Created on behalf of the IFRA Group at Cranfield University, United Kingdom          #
+#  E-mail: IFRA@cranfield.ac.uk                                                         #
+#                                                                                       #
+#  Licensed under the Apache-2.0 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.                    #
+#                                                                                       #
+#  IFRA Group - Cranfield University                                                    #
+#  AUTHORS: Mikel Bueno Viso - Mikel.Bueno-Viso@cranfield.ac.uk                         #
+#           Dr. Seemal Asif  - s.asif@cranfield.ac.uk                                   #
+#           Prof. Phil Webb  - p.f.webb@cranfield.ac.uk                                 #
+#                                                                                       #
+#  Date: May, 2023.                                                                     #
+#                                                                                       #
+# ===================================== COPYRIGHT ===================================== #
+
+# ======= CITE OUR WORK ======= #
+# You can cite our work with the following statement:
+# IFRA-Cranfield (2023) ROS 2 Sim-to-Real Robot Control. URL: https://github.com/IFRA-Cranfield/ros2_SimRealRobotControl.
+
+-->
+
+<sdf version="1.4">
+
+  <world name="default">
+
+    <!-- WORLD COMPONENTS: STANDARD -->
+
+    <gravity>0 0 -9.8</gravity>
+
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <scene>
+      <shadows>0</shadows>
+    </scene>
+
+    <!-- Link Attacher: GAZEBO PLUGIN -->
+    <plugin name="gazebo_link_attacher" filename="libgazebo_link_attacher.so"/>
+
+  </world>
+</sdf>
diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/CMakeLists.txt b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/CMakeLists.txt
new file mode 100644
index 000000000..6e9f6ade0
--- /dev/null
+++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/CMakeLists.txt
@@ -0,0 +1,43 @@
+cmake_minimum_required(VERSION 3.5)
+project(ros2srrc_ur3_moveit2)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+# REQUIRED to -> Launch simulation:
+install(
+  DIRECTORY 
+    config
+  DESTINATION 
+    share/${PROJECT_NAME}
+)
+
+ament_package()
diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3.srdf b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3.srdf
new file mode 100644
index 000000000..6aabb088e
--- /dev/null
+++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3.srdf
@@ -0,0 +1,73 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<!-- 
+
+# ===================================== COPYRIGHT ===================================== #
+#                                                                                       #
+#  IFRA (Intelligent Flexible Robotics and Assembly) Group, CRANFIELD UNIVERSITY        #
+#  Created on behalf of the IFRA Group at Cranfield University, United Kingdom          #
+#  E-mail: IFRA@cranfield.ac.uk                                                         #
+#                                                                                       #
+#  Licensed under the Apache-2.0 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.                    #
+#                                                                                       #
+#  IFRA Group - Cranfield University                                                    #
+#  AUTHORS: Mikel Bueno Viso - Mikel.Bueno-Viso@cranfield.ac.uk                         #
+#           Dr. Seemal Asif  - s.asif@cranfield.ac.uk                                   #
+#           Prof. Phil Webb  - p.f.webb@cranfield.ac.uk                                 #
+#                                                                                       #
+#  Date: April, 2023.                                                                   #
+#                                                                                       #
+# ===================================== COPYRIGHT ===================================== #
+
+# ======= CITE OUR WORK ======= #
+# You can cite our work with the following statement:
+# IFRA-Cranfield (2023) ROS 2 Sim-to-Real Robot Control. URL: https://github.com/IFRA-Cranfield/ros2_SimRealRobotControl.
+
+-->
+
+<robot name="ur3">
+
+  <group name="ur3_arm">
+    <chain base_link="base_link" tip_link="tool0"/>
+  </group>
+
+  <group_state group="ur3_arm" name="home">
+    <joint name="elbow_joint" value="0" />
+    <joint name="shoulder_lift_joint" value="-1.5707" />
+    <joint name="shoulder_pan_joint" value="0" />
+    <joint name="wrist_1_joint" value="0" />
+    <joint name="wrist_2_joint" value="0" />
+    <joint name="wrist_3_joint" value="0" />
+  </group_state>
+
+  <group_state group="ur3_arm" name="up">
+    <joint name="elbow_joint" value="0" />
+    <joint name="shoulder_lift_joint" value="-1.5707" />
+    <joint name="shoulder_pan_joint" value="0" />
+    <joint name="wrist_1_joint" value="-1.5707" />
+    <joint name="wrist_2_joint" value="0" />
+    <joint name="wrist_3_joint" value="0" />
+  </group_state>
+
+  <virtual_joint child_link="base_link" name="virtual_joint" parent_frame="world" type="fixed"/>
+
+  <disable_collisions link1="base_link" link2="base_link_inertia" reason="Adjacent" />
+  <disable_collisions link1="base_link_inertia" link2="shoulder_link" reason="Adjacent" />
+  <disable_collisions link1="tool0" link2="wrist_1_link" reason="Never" />
+  <disable_collisions link1="tool0" link2="wrist_2_link" reason="Never" />
+  <disable_collisions link1="tool0" link2="wrist_3_link" reason="Adjacent" />
+  <disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent" />
+  <disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent" />
+  <disable_collisions link1="shoulder_link" link2="upper_arm_link" reason="Adjacent" />
+  <disable_collisions link1="wrist_1_link" link2="wrist_2_link" reason="Adjacent" />
+  <disable_collisions link1="wrist_1_link" link2="wrist_3_link" reason="Never" />
+  <disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent" />
+
+</robot>
\ No newline at end of file
diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3_moveit2.rviz b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3_moveit2.rviz
new file mode 100644
index 000000000..bdaee0371
--- /dev/null
+++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3_moveit2.rviz
@@ -0,0 +1,318 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded: ~
+      Splitter Ratio: 0.5
+    Tree Height: 348
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Acceleration_Scaling_Factor: 0.7
+      Class: moveit_rviz_plugin/MotionPlanning
+      Enabled: true
+      Move Group Namespace: ""
+      MoveIt_Allow_Approximate_IK: false
+      MoveIt_Allow_External_Program: false
+      MoveIt_Allow_Replanning: false
+      MoveIt_Allow_Sensor_Positioning: false
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Constraint_Aware_IK: false
+      MoveIt_Workspace:
+        Center:
+          X: 0
+          Y: 0
+          Z: 0
+        Size:
+          X: 2
+          Y: 2
+          Z: 2
+      Name: MotionPlanning
+      Planned Path:
+        Color Enabled: false
+        Interrupt Display: false
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link_inertia:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          flange:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          forearm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          shoulder_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tool0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          upper_arm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Loop Animation: false
+        Robot Alpha: 0.5
+        Robot Color: 150; 50; 150
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Show Trail: false
+        State Display Time: 3x
+        Trail Step Size: 1
+        Trajectory Topic: /display_planned_path
+      Planning Metrics:
+        Payload: 1
+        Show Joint Torques: false
+        Show Manipulability: false
+        Show Manipulability Index: false
+        Show Weight Limit: false
+        TextHeight: 0.07999999821186066
+      Planning Request:
+        Colliding Link Color: 255; 0; 0
+        Goal State Alpha: 1
+        Goal State Color: 250; 128; 0
+        Interactive Marker Size: 0
+        Joint Violation Color: 255; 0; 255
+        Planning Group: manipulator
+        Query Goal State: true
+        Query Start State: false
+        Show Workspace: false
+        Start State Alpha: 1
+        Start State Color: 0; 255; 0
+      Planning Scene Topic: /monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.009999999776482582
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link_inertia:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          flange:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          forearm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          shoulder_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tool0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          upper_arm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: true
+      Velocity_Scaling_Factor: 0.7
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 10
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.785398006439209
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.785398006439209
+    Saved: ~
+Window Geometry:
+  "":
+    collapsed: false
+  " - Trajectory Slider":
+    collapsed: false
+  Displays:
+    collapsed: false
+  Height: 1043
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f300000375fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001e7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffbffffffff010000022a000001880000017d00ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005870000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1920
+  X: 0
+  Y: 0
\ No newline at end of file
diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85.srdf b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85.srdf
new file mode 100644
index 000000000..eb5f1a064
--- /dev/null
+++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85.srdf
@@ -0,0 +1,219 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<!-- 
+
+# ===================================== COPYRIGHT ===================================== #
+#                                                                                       #
+#  IFRA (Intelligent Flexible Robotics and Assembly) Group, CRANFIELD UNIVERSITY        #
+#  Created on behalf of the IFRA Group at Cranfield University, United Kingdom          #
+#  E-mail: IFRA@cranfield.ac.uk                                                         #
+#                                                                                       #
+#  Licensed under the Apache-2.0 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.                    #
+#                                                                                       #
+#  IFRA Group - Cranfield University                                                    #
+#  AUTHORS: Mikel Bueno Viso - Mikel.Bueno-Viso@cranfield.ac.uk                         #
+#           Dr. Seemal Asif  - s.asif@cranfield.ac.uk                                   #
+#           Prof. Phil Webb  - p.f.webb@cranfield.ac.uk                                 #
+#                                                                                       #
+#  Date: April, 2023.                                                                   #
+#                                                                                       #
+# ===================================== COPYRIGHT ===================================== #
+
+# ======= CITE OUR WORK ======= #
+# You can cite our work with the following statement:
+# IFRA-Cranfield (2023) ROS 2 Sim-to-Real Robot Control. URL: https://github.com/IFRA-Cranfield/ros2_SimRealRobotControl.
+
+-->
+
+<robot name="ur3">
+
+  <group name="ur3_arm">
+    <chain base_link="base_link" tip_link="tool0"/>
+  </group>
+
+  <group name="robotiq_2f85">
+    <joint name="robotiq_85_left_knuckle_joint"/>
+    <joint name="robotiq_85_right_knuckle_joint"/>
+    <joint name="robotiq_85_left_inner_knuckle_joint"/>
+    <joint name="robotiq_85_right_inner_knuckle_joint"/>
+    <joint name="robotiq_85_left_finger_tip_joint"/>
+    <joint name="robotiq_85_right_finger_tip_joint"/>
+  </group>
+
+  <group_state group="ur3_arm" name="home">
+    <joint name="elbow_joint" value="0" />
+    <joint name="shoulder_lift_joint" value="-1.5707" />
+    <joint name="shoulder_pan_joint" value="0" />
+    <joint name="wrist_1_joint" value="0" />
+    <joint name="wrist_2_joint" value="0" />
+    <joint name="wrist_3_joint" value="0" />
+  </group_state>
+
+  <group_state group="ur3_arm" name="up">
+    <joint name="elbow_joint" value="0" />
+    <joint name="shoulder_lift_joint" value="-1.5707" />
+    <joint name="shoulder_pan_joint" value="0" />
+    <joint name="wrist_1_joint" value="-1.5707" />
+    <joint name="wrist_2_joint" value="0" />
+    <joint name="wrist_3_joint" value="0" />
+  </group_state>
+
+  <group_state name="GripperOpen" group="robotiq_2f85">
+    <joint name="robotiq_85_left_knuckle_joint" value="0" />
+    <joint name="robotiq_85_right_knuckle_joint" value="0" />
+    <joint name="robotiq_85_left_inner_knuckle_joint" value="0" />
+    <joint name="robotiq_85_right_inner_knuckle_joint" value="0" />
+    <joint name="robotiq_85_left_finger_tip_joint" value="0" />
+    <joint name="robotiq_85_right_finger_tip_joint" value="0" />
+  </group_state>
+
+  <group_state name="GripperCloseHALF" group="robotiq_2f85">
+    <joint name="robotiq_85_left_knuckle_joint" value="0.5" />
+    <joint name="robotiq_85_right_knuckle_joint" value="0.5" />
+    <joint name="robotiq_85_left_inner_knuckle_joint" value="0.5" />
+    <joint name="robotiq_85_right_inner_knuckle_joint" value="0.5" />
+    <joint name="robotiq_85_left_finger_tip_joint" value="-0.5" />
+    <joint name="robotiq_85_right_finger_tip_joint" value="-0.5" />
+  </group_state>
+
+  <group_state name="GripperCloseFULL" group="robotiq_2f85">
+    <joint name="robotiq_85_left_knuckle_joint" value="0.8" />
+    <joint name="robotiq_85_right_knuckle_joint" value="0.8" />
+    <joint name="robotiq_85_left_inner_knuckle_joint" value="0.8" />
+    <joint name="robotiq_85_right_inner_knuckle_joint" value="0.8" />
+    <joint name="robotiq_85_left_finger_tip_joint" value="-0.8" />
+    <joint name="robotiq_85_right_finger_tip_joint" value="-0.8" />
+  </group_state>
+
+  <virtual_joint child_link="base_link" name="virtual_joint" parent_frame="world" type="fixed"/>
+
+  <disable_collisions link1="base_link" link2="base_link_inertia" reason="Adjacent" />
+  <disable_collisions link1="base_link_inertia" link2="shoulder_link" reason="Adjacent" />
+  <disable_collisions link1="tool0" link2="wrist_1_link" reason="Never" />
+  <disable_collisions link1="tool0" link2="wrist_2_link" reason="Never" />
+  <disable_collisions link1="tool0" link2="wrist_3_link" reason="Adjacent" />
+  <disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent" />
+  <disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent" />
+  <disable_collisions link1="shoulder_link" link2="upper_arm_link" reason="Adjacent" />
+  <disable_collisions link1="wrist_1_link" link2="wrist_2_link" reason="Adjacent" />
+  <disable_collisions link1="wrist_1_link" link2="wrist_3_link" reason="Never" />
+  <disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent" />
+
+  <disable_collisions link1="tool0" link2="robotiq_85_base_link" reason="Never" />
+  <disable_collisions link1="tool0" link2="robotiq_85_left_finger_link" reason="Never" />
+  <disable_collisions link1="tool0" link2="robotiq_85_left_finger_tip_link" reason="Never" />
+  <disable_collisions link1="tool0" link2="robotiq_85_left_inner_knuckle_link" reason="Never" />
+  <disable_collisions link1="tool0" link2="robotiq_85_left_knuckle_link" reason="Never" />
+  <disable_collisions link1="tool0" link2="robotiq_85_right_finger_link" reason="Never" />
+  <disable_collisions link1="tool0" link2="robotiq_85_right_finger_tip_link" reason="Never" />
+  <disable_collisions link1="tool0" link2="robotiq_85_right_inner_knuckle_link" reason="Never" />
+  <disable_collisions link1="tool0" link2="robotiq_85_right_knuckle_link" reason="Never" />
+  <disable_collisions link1="tool0" link2="robotiq_coupler" reason="Never" />
+
+  <disable_collisions link1="robotiq_85_base_link" link2="robotiq_85_left_finger_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_base_link" link2="robotiq_85_left_finger_tip_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_base_link" link2="robotiq_85_left_inner_knuckle_link" reason="Adjacent" />
+  <disable_collisions link1="robotiq_85_base_link" link2="robotiq_85_left_knuckle_link" reason="Adjacent" />
+  <disable_collisions link1="robotiq_85_base_link" link2="robotiq_85_right_finger_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_base_link" link2="robotiq_85_right_finger_tip_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_base_link" link2="robotiq_85_right_inner_knuckle_link" reason="Adjacent" />
+  <disable_collisions link1="robotiq_85_base_link" link2="robotiq_85_right_knuckle_link" reason="Adjacent" />
+  <disable_collisions link1="robotiq_85_base_link" link2="robotiq_coupler" reason="Adjacent" />
+  <disable_collisions link1="robotiq_85_base_link" link2="wrist_1_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_base_link" link2="wrist_2_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_base_link" link2="wrist_3_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_finger_link" link2="robotiq_85_left_finger_tip_link" reason="Default" />
+  <disable_collisions link1="robotiq_85_left_finger_link" link2="robotiq_85_left_inner_knuckle_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_finger_link" link2="robotiq_85_left_knuckle_link" reason="Adjacent" />
+  <disable_collisions link1="robotiq_85_left_finger_link" link2="robotiq_85_right_finger_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_finger_link" link2="robotiq_85_right_finger_tip_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_finger_link" link2="robotiq_85_right_inner_knuckle_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_finger_link" link2="robotiq_85_right_knuckle_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_finger_link" link2="robotiq_coupler" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_finger_link" link2="wrist_1_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_finger_link" link2="wrist_2_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_finger_link" link2="wrist_3_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_finger_tip_link" link2="robotiq_85_left_inner_knuckle_link" reason="Adjacent" />
+  <disable_collisions link1="robotiq_85_left_finger_tip_link" link2="robotiq_85_left_knuckle_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_finger_tip_link" link2="robotiq_85_right_finger_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_finger_tip_link" link2="robotiq_85_right_finger_tip_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_finger_tip_link" link2="robotiq_85_right_inner_knuckle_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_finger_tip_link" link2="robotiq_85_right_knuckle_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_finger_tip_link" link2="robotiq_coupler" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_finger_tip_link" link2="wrist_1_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_finger_tip_link" link2="wrist_2_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_finger_tip_link" link2="wrist_3_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_inner_knuckle_link" link2="robotiq_85_left_knuckle_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_inner_knuckle_link" link2="robotiq_85_right_finger_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_inner_knuckle_link" link2="robotiq_85_right_finger_tip_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_inner_knuckle_link" link2="robotiq_85_right_inner_knuckle_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_inner_knuckle_link" link2="robotiq_85_right_knuckle_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_inner_knuckle_link" link2="robotiq_coupler" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_inner_knuckle_link" link2="wrist_1_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_inner_knuckle_link" link2="wrist_2_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_inner_knuckle_link" link2="wrist_3_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_knuckle_link" link2="robotiq_85_right_finger_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_knuckle_link" link2="robotiq_85_right_finger_tip_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_knuckle_link" link2="robotiq_85_right_inner_knuckle_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_knuckle_link" link2="robotiq_85_right_knuckle_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_knuckle_link" link2="robotiq_coupler" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_knuckle_link" link2="wrist_1_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_knuckle_link" link2="wrist_2_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_left_knuckle_link" link2="wrist_3_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_right_finger_link" link2="robotiq_85_right_finger_tip_link" reason="Default" />
+  <disable_collisions link1="robotiq_85_right_finger_link" link2="robotiq_85_right_inner_knuckle_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_right_finger_link" link2="robotiq_85_right_knuckle_link" reason="Adjacent" />
+  <disable_collisions link1="robotiq_85_right_finger_link" link2="robotiq_coupler" reason="Never" />
+  <disable_collisions link1="robotiq_85_right_finger_link" link2="wrist_1_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_right_finger_link" link2="wrist_2_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_right_finger_link" link2="wrist_3_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_right_finger_tip_link" link2="robotiq_85_right_inner_knuckle_link" reason="Adjacent" />
+  <disable_collisions link1="robotiq_85_right_finger_tip_link" link2="robotiq_85_right_knuckle_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_right_finger_tip_link" link2="robotiq_coupler" reason="Never" />
+  <disable_collisions link1="robotiq_85_right_finger_tip_link" link2="wrist_1_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_right_finger_tip_link" link2="wrist_2_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_right_finger_tip_link" link2="wrist_3_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_right_inner_knuckle_link" link2="robotiq_85_right_knuckle_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_right_inner_knuckle_link" link2="robotiq_coupler" reason="Never" />
+  <disable_collisions link1="robotiq_85_right_inner_knuckle_link" link2="wrist_1_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_right_inner_knuckle_link" link2="wrist_2_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_right_inner_knuckle_link" link2="wrist_3_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_right_knuckle_link" link2="robotiq_coupler" reason="Never" />
+  <disable_collisions link1="robotiq_85_right_knuckle_link" link2="wrist_1_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_right_knuckle_link" link2="wrist_2_link" reason="Never" />
+  <disable_collisions link1="robotiq_85_right_knuckle_link" link2="wrist_3_link" reason="Never" />
+  <disable_collisions link1="robotiq_coupler" link2="wrist_1_link" reason="Never" />
+  <disable_collisions link1="robotiq_coupler" link2="wrist_2_link" reason="Never" />
+  <disable_collisions link1="robotiq_coupler" link2="wrist_3_link" reason="Adjacent" />
+  <disable_collisions link1="hand_camera_link" link2="tool0" reason="Never"/>
+  <disable_collisions link1="hand_camera_link" link2="robotiq_coupler" reason="Never"/>
+  <disable_collisions link1="hand_camera_link" link2="robotiq_85_base_link" reason="Never"/>
+  <disable_collisions link1="hand_camera_link" link2="robotiq_85_left_knuckle_link" reason="Never"/>
+  <disable_collisions link1="hand_camera_link" link2="robotiq_85_right_knuckle_link" reason="Never"/>
+  <disable_collisions link1="hand_camera_link" link2="wrist_3_link" reason="Never"/>
+
+  <!-- UR3 + Robotiq -->
+  <disable_collisions link1="forearm_link" link2="robotiq_coupler" reason="Never"/>
+
+  <disable_collisions link1="forearm_link" link2="robotiq_85_base_link" reason="Never"/>
+
+  <disable_collisions link1="forearm_link" link2="robotiq_85_left_knuckle_link" reason="Never"/>
+  <disable_collisions link1="forearm_link" link2="robotiq_85_right_knuckle_link" reason="Never"/>
+
+  <disable_collisions link1="forearm_link" link2="robotiq_85_left_inner_knuckle_link" reason="Never"/>
+  <disable_collisions link1="forearm_link" link2="robotiq_85_right_inner_knuckle_link" reason="Never"/>
+
+  <disable_collisions link1="forearm_link" link2="robotiq_85_left_finger_link" reason="Never"/>
+  <disable_collisions link1="forearm_link" link2="robotiq_85_right_finger_link" reason="Never"/>
+
+  <disable_collisions link1="forearm_link" link2="robotiq_85_left_finger_tip_link" reason="Never"/>
+  <disable_collisions link1="forearm_link" link2="robotiq_85_right_finger_tip_link" reason="Never"/>
+
+</robot>
\ No newline at end of file
diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85_moveit2.rviz b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85_moveit2.rviz
new file mode 100644
index 000000000..1f96d17f7
--- /dev/null
+++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_2f85_moveit2.rviz
@@ -0,0 +1,363 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded: ~
+      Splitter Ratio: 0.5
+    Tree Height: 348
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Acceleration_Scaling_Factor: 0.7
+      Class: moveit_rviz_plugin/MotionPlanning
+      Enabled: true
+      Move Group Namespace: ""
+      MoveIt_Allow_Approximate_IK: false
+      MoveIt_Allow_External_Program: false
+      MoveIt_Allow_Replanning: false
+      MoveIt_Allow_Sensor_Positioning: false
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Constraint_Aware_IK: false
+      MoveIt_Workspace:
+        Center:
+          X: 0
+          Y: 0
+          Z: 0
+        Size:
+          X: 2
+          Y: 2
+          Z: 2
+      Name: MotionPlanning
+      Planned Path:
+        Color Enabled: false
+        Interrupt Display: false
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link_inertia:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          flange:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          forearm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          robotiq_85_base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          robotiq_85_left_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          robotiq_85_right_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          robotiq_85_left_finger_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          robotiq_85_right_finger_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          robotiq_85_left_inner_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          robotiq_85_right_inner_knuckle_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          robotiq_85_left_finger_tip_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          robotiq_85_right_finger_tip_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          shoulder_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tool0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          upper_arm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Loop Animation: false
+        Robot Alpha: 0.5
+        Robot Color: 150; 50; 150
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Show Trail: false
+        State Display Time: 3x
+        Trail Step Size: 1
+        Trajectory Topic: /display_planned_path
+      Planning Metrics:
+        Payload: 1
+        Show Joint Torques: false
+        Show Manipulability: false
+        Show Manipulability Index: false
+        Show Weight Limit: false
+        TextHeight: 0.07999999821186066
+      Planning Request:
+        Colliding Link Color: 255; 0; 0
+        Goal State Alpha: 1
+        Goal State Color: 250; 128; 0
+        Interactive Marker Size: 0
+        Joint Violation Color: 255; 0; 255
+        Planning Group: manipulator
+        Query Goal State: true
+        Query Start State: false
+        Show Workspace: false
+        Start State Alpha: 1
+        Start State Color: 0; 255; 0
+      Planning Scene Topic: /monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.009999999776482582
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link_inertia:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          flange:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          forearm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          shoulder_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tool0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          upper_arm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: true
+      Velocity_Scaling_Factor: 0.7
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 10
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.785398006439209
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.785398006439209
+    Saved: ~
+Window Geometry:
+  "":
+    collapsed: false
+  " - Trajectory Slider":
+    collapsed: false
+  Displays:
+    collapsed: false
+  Height: 1043
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f300000375fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001e7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffbffffffff010000022a000001880000017d00ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005870000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1920
+  X: 0
+  Y: 0
\ No newline at end of file
diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_hande.srdf b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_hande.srdf
new file mode 100644
index 000000000..a7f70bc79
--- /dev/null
+++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_hande.srdf
@@ -0,0 +1,135 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<!-- 
+
+# ===================================== COPYRIGHT ===================================== #
+#                                                                                       #
+#  IFRA (Intelligent Flexible Robotics and Assembly) Group, CRANFIELD UNIVERSITY        #
+#  Created on behalf of the IFRA Group at Cranfield University, United Kingdom          #
+#  E-mail: IFRA@cranfield.ac.uk                                                         #
+#                                                                                       #
+#  Licensed under the Apache-2.0 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.                    #
+#                                                                                       #
+#  IFRA Group - Cranfield University                                                    #
+#  AUTHORS: Mikel Bueno Viso - Mikel.Bueno-Viso@cranfield.ac.uk                         #
+#           Dr. Seemal Asif  - s.asif@cranfield.ac.uk                                   #
+#           Prof. Phil Webb  - p.f.webb@cranfield.ac.uk                                 #
+#                                                                                       #
+#  Date: April, 2023.                                                                   #
+#                                                                                       #
+# ===================================== COPYRIGHT ===================================== #
+
+# ======= CITE OUR WORK ======= #
+# You can cite our work with the following statement:
+# IFRA-Cranfield (2023) ROS 2 Sim-to-Real Robot Control. URL: https://github.com/IFRA-Cranfield/ros2_SimRealRobotControl.
+
+-->
+
+<robot name="ur3">
+
+  <group name="ur3_arm">
+    <chain base_link="base_link" tip_link="EE_robotiq_hande"/>
+  </group>
+
+  <group name="robotiq_hande">
+  
+    <joint name="robotiq_bracket_joint" />
+    <joint name="robotiq_coupler_joint" />
+    <joint name="robotiq_hande_base_joint" />
+
+    <joint name="robotiq_hande_left_finger_joint" />
+    <joint name="robotiq_hande_right_finger_joint" />
+    
+  </group>
+
+  <group_state group="ur3_arm" name="home">
+    <joint name="elbow_joint" value="0" />
+    <joint name="shoulder_lift_joint" value="-1.5707" />
+    <joint name="shoulder_pan_joint" value="0" />
+    <joint name="wrist_1_joint" value="0" />
+    <joint name="wrist_2_joint" value="0" />
+    <joint name="wrist_3_joint" value="0" />
+  </group_state>
+
+  <group_state group="ur3_arm" name="up">
+    <joint name="elbow_joint" value="0" />
+    <joint name="shoulder_lift_joint" value="-1.5707" />
+    <joint name="shoulder_pan_joint" value="0" />
+    <joint name="wrist_1_joint" value="-1.5707" />
+    <joint name="wrist_2_joint" value="0" />
+    <joint name="wrist_3_joint" value="0" />
+  </group_state>
+
+  <group_state name="GripperOpen" group="robotiq_hande">
+    <joint name="robotiq_hande_left_finger_joint" value="0" />
+    <joint name="robotiq_hande_right_finger_joint" value="0" />
+  </group_state>
+
+  <group_state name="GripperCloseHALF" group="robotiq_hande">
+    <joint name="robotiq_hande_left_finger_joint" value="0.0125" />
+    <joint name="robotiq_hande_right_finger_joint" value="0.0125" />
+  </group_state>
+
+  <group_state name="GripperCloseFULL" group="robotiq_hande">
+    <joint name="robotiq_hande_left_finger_joint" value="0.025" />
+    <joint name="robotiq_hande_right_finger_joint" value="0.025" />
+  </group_state>
+
+  <virtual_joint child_link="base_link" name="virtual_joint" parent_frame="world" type="fixed"/>
+
+  <disable_collisions link1="base_link" link2="base_link_inertia" reason="Adjacent" />
+  <disable_collisions link1="base_link_inertia" link2="shoulder_link" reason="Adjacent" />
+  <disable_collisions link1="tool0" link2="wrist_1_link" reason="Never" />
+  <disable_collisions link1="tool0" link2="wrist_2_link" reason="Never" />
+  <disable_collisions link1="tool0" link2="wrist_3_link" reason="Adjacent" />
+  <disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent" />
+  <disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent" />
+  <disable_collisions link1="shoulder_link" link2="upper_arm_link" reason="Adjacent" />
+  <disable_collisions link1="wrist_1_link" link2="wrist_2_link" reason="Adjacent" />
+  <disable_collisions link1="wrist_1_link" link2="wrist_3_link" reason="Never" />
+  <disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent" />
+
+  <disable_collisions link1="tool0" link2="robotiq_hande_base_link" reason="Never" />
+  <disable_collisions link1="tool0" link2="robotiq_hande_left_finger" reason="Never" />
+  <disable_collisions link1="tool0" link2="robotiq_hande_right_finger" reason="Never" />
+  <disable_collisions link1="tool0" link2="robotiq_bracket" reason="Never" />
+  <disable_collisions link1="tool0" link2="robotiq_coupler" reason="Never" />
+
+  <disable_collisions link1="robotiq_hande_base_link" link2="robotiq_hande_left_finger" reason="Never" />
+  <disable_collisions link1="robotiq_hande_base_link" link2="robotiq_hande_right_finger" reason="Never" />
+  <disable_collisions link1="robotiq_hande_base_link" link2="robotiq_coupler" reason="Adjacent" />
+  <disable_collisions link1="robotiq_hande_base_link" link2="robotiq_bracket" reason="Never" />
+  <disable_collisions link1="robotiq_hande_base_link" link2="wrist_1_link" reason="Never" />
+  <disable_collisions link1="robotiq_hande_base_link" link2="wrist_2_link" reason="Never" />
+  <disable_collisions link1="robotiq_hande_base_link" link2="wrist_3_link" reason="Never" />
+
+  <disable_collisions link1="robotiq_hande_left_finger" link2="robotiq_hande_right_finger" reason="Never" />
+  <disable_collisions link1="robotiq_hande_left_finger" link2="robotiq_coupler" reason="Never" />
+  <disable_collisions link1="robotiq_hande_left_finger" link2="robotiq_bracket" reason="Never" />
+  <disable_collisions link1="robotiq_hande_left_finger" link2="wrist_1_link" reason="Never" />
+  <disable_collisions link1="robotiq_hande_left_finger" link2="wrist_2_link" reason="Never" />
+  <disable_collisions link1="robotiq_hande_left_finger" link2="wrist_3_link" reason="Never" />
+
+  <disable_collisions link1="robotiq_hande_right_finger" link2="robotiq_hande_left_finger" reason="Never" />
+  <disable_collisions link1="robotiq_hande_right_finger" link2="robotiq_coupler" reason="Never" />
+  <disable_collisions link1="robotiq_hande_right_finger" link2="robotiq_bracket" reason="Never" />
+  <disable_collisions link1="robotiq_hande_right_finger" link2="wrist_1_link" reason="Never" />
+  <disable_collisions link1="robotiq_hande_right_finger" link2="wrist_2_link" reason="Never" />
+  <disable_collisions link1="robotiq_hande_right_finger" link2="wrist_3_link" reason="Never" />
+
+  <disable_collisions link1="robotiq_coupler" link2="wrist_1_link" reason="Never" />
+  <disable_collisions link1="robotiq_coupler" link2="wrist_2_link" reason="Never" />
+  <disable_collisions link1="robotiq_coupler" link2="wrist_3_link" reason="Adjacent" />
+
+  <disable_collisions link1="robotiq_bracket" link2="wrist_1_link" reason="Never" />
+  <disable_collisions link1="robotiq_bracket" link2="wrist_2_link" reason="Never" />
+  <disable_collisions link1="robotiq_bracket" link2="wrist_3_link" reason="Adjacent" />
+
+</robot>
\ No newline at end of file
diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_hande_moveit2.rviz b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_hande_moveit2.rviz
new file mode 100644
index 000000000..aca1caf2d
--- /dev/null
+++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/config/ur3robotiq_hande_moveit2.rviz
@@ -0,0 +1,333 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded: ~
+      Splitter Ratio: 0.5
+    Tree Height: 348
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Acceleration_Scaling_Factor: 0.7
+      Class: moveit_rviz_plugin/MotionPlanning
+      Enabled: true
+      Move Group Namespace: ""
+      MoveIt_Allow_Approximate_IK: false
+      MoveIt_Allow_External_Program: false
+      MoveIt_Allow_Replanning: false
+      MoveIt_Allow_Sensor_Positioning: false
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Constraint_Aware_IK: false
+      MoveIt_Workspace:
+        Center:
+          X: 0
+          Y: 0
+          Z: 0
+        Size:
+          X: 2
+          Y: 2
+          Z: 2
+      Name: MotionPlanning
+      Planned Path:
+        Color Enabled: false
+        Interrupt Display: false
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link_inertia:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          flange:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          forearm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          robotiq_hande_base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          robotiq_hande_left_finger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          robotiq_hande_right_finger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          shoulder_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tool0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          upper_arm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Loop Animation: false
+        Robot Alpha: 0.5
+        Robot Color: 150; 50; 150
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Show Trail: false
+        State Display Time: 3x
+        Trail Step Size: 1
+        Trajectory Topic: /display_planned_path
+      Planning Metrics:
+        Payload: 1
+        Show Joint Torques: false
+        Show Manipulability: false
+        Show Manipulability Index: false
+        Show Weight Limit: false
+        TextHeight: 0.07999999821186066
+      Planning Request:
+        Colliding Link Color: 255; 0; 0
+        Goal State Alpha: 1
+        Goal State Color: 250; 128; 0
+        Interactive Marker Size: 0
+        Joint Violation Color: 255; 0; 255
+        Planning Group: manipulator
+        Query Goal State: true
+        Query Start State: false
+        Show Workspace: false
+        Start State Alpha: 1
+        Start State Color: 0; 255; 0
+      Planning Scene Topic: /monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.009999999776482582
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link_inertia:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          flange:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          forearm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          shoulder_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tool0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          upper_arm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: true
+      Velocity_Scaling_Factor: 0.7
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 10
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.785398006439209
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.785398006439209
+    Saved: ~
+Window Geometry:
+  "":
+    collapsed: false
+  " - Trajectory Slider":
+    collapsed: false
+  Displays:
+    collapsed: false
+  Height: 1043
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f300000375fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001e7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffbffffffff010000022a000001880000017d00ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005870000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1920
+  X: 0
+  Y: 0
\ No newline at end of file
diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/package.xml b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/package.xml
new file mode 100644
index 000000000..7f15f5caa
--- /dev/null
+++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/package.xml
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ros2srrc_ur3_moveit2</name>
+  <version>0.0.0</version>
+  <description>IFRA-Cranfield (Cranfield University, UK). Package: ROS 2 SimRealRobotControl - UR3-MoveIt!2</description>
+
+  <author>Mikel Bueno Viso</author>
+  <maintainer email="Mikel.Bueno-Viso@cranfield.ac.uk">Mikel Bueno Viso</maintainer>
+  
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/rviz/moveit.rviz b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/rviz/moveit.rviz
new file mode 100644
index 000000000..c8092de74
--- /dev/null
+++ b/Industrial/ros2_SimRealRobotControl_gz/packages/ur3/ros2srrc_ur3_moveit2/rviz/moveit.rviz
@@ -0,0 +1,311 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 87
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+      Splitter Ratio: 0.4957627058029175
+    Tree Height: 573
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Acceleration_Scaling_Factor: 0.1
+      Class: moveit_rviz_plugin/MotionPlanning
+      Enabled: true
+      Move Group Namespace: ""
+      MoveIt_Allow_Approximate_IK: false
+      MoveIt_Allow_External_Program: false
+      MoveIt_Allow_Replanning: false
+      MoveIt_Allow_Sensor_Positioning: false
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Constraint_Aware_IK: false
+      MoveIt_Warehouse_Host: 127.0.0.1
+      MoveIt_Warehouse_Port: 33829
+      MoveIt_Workspace:
+        Center:
+          X: 0
+          Y: 0
+          Z: 0
+        Size:
+          X: 2
+          Y: 2
+          Z: 2
+      Name: MotionPlanning
+      Planned Path:
+        Color Enabled: true
+        Interrupt Display: true
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link_inertia:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          flange:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          forearm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          shoulder_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tool0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          upper_arm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Loop Animation: false
+        Robot Alpha: 0.5
+        Robot Color: 150; 50; 150
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Show Trail: false
+        State Display Time: 0.05 s
+        Trail Step Size: 1
+        Trajectory Topic: /display_planned_path
+      Planning Metrics:
+        Payload: 1
+        Show Joint Torques: false
+        Show Manipulability: false
+        Show Manipulability Index: false
+        Show Weight Limit: false
+        TextHeight: 0.07999999821186066
+      Planning Request:
+        Colliding Link Color: 255; 0; 0
+        Goal State Alpha: 1
+        Goal State Color: 250; 128; 0
+        Interactive Marker Size: 0.3
+        Joint Violation Color: 255; 0; 255
+        Planning Group: ur3_arm
+        Query Goal State: true
+        Query Start State: false
+        Show Workspace: false
+        Start State Alpha: 1
+        Start State Color: 0; 255; 0
+      Planning Scene Topic: monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.009999999776482582
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          base:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          base_link_inertia:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          flange:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          forearm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          shoulder_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tool0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          upper_arm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_1_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_2_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_3_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: true
+      Velocity_Scaling_Factor: 0.1
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 6.619869709014893
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.2489434778690338
+        Y: -0.013962505385279655
+        Z: 0.13800443708896637
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.4103981554508209
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 1.210397720336914
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1381
+  Hide Left Dock: false
+  Hide Right Dock: false
+  MotionPlanning:
+    collapsed: false
+  MotionPlanning - Trajectory Slider:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001e0000004f6fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000046000002e3000000ff01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000005201000003fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032a00000212000001b9010000030000000100000110000004f6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000046000004f6000000d701000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000070e000004f600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 2560
+  X: 0
+  Y: 30
diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_macro.urdf.xacro b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_macro.urdf.xacro
index 76a695acf..9a5f56205 100644
--- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_macro.urdf.xacro
+++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_macro.urdf.xacro
@@ -1,86 +1,18 @@
 <?xml version="1.0"?>
 
-<!-- 
+<robot name="robotiq_85_gripper" xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-# ===================================== COPYRIGHT ===================================== #
-#                                                                                       #
-#  IFRA (Intelligent Flexible Robotics and Assembly) Group, CRANFIELD UNIVERSITY        #
-#  Created on behalf of the IFRA Group at Cranfield University, United Kingdom          #
-#  E-mail: IFRA@cranfield.ac.uk                                                         #
-#                                                                                       #
-#  Licensed under the Apache-2.0 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.                    #
-#                                                                                       #
-#  IFRA Group - Cranfield University                                                    #
-#  AUTHORS: Mikel Bueno Viso - Mikel.Bueno-Viso@cranfield.ac.uk                         #
-#           Dr. Seemal Asif  - s.asif@cranfield.ac.uk                                   #
-#           Prof. Phil Webb  - p.f.webb@cranfield.ac.uk                                 #
-#                                                                                       #
-#  Date: April, 2023.                                                                   #
-#                                                                                       #
-# ===================================== COPYRIGHT ===================================== #
+  <xacro:include filename="$(find robotiq_description)/urdf/robotiq_85_gripper.transmission.xacro" />
+  
+  <xacro:macro name="robotiq_85_gripper" params="prefix parent *origin">
 
-# ======= CITE OUR WORK ======= #
-# You can cite our work with the following statement:
-# IFRA-Cranfield (2023) ROS 2 Sim-to-Real Robot Control. URL: https://github.com/IFRA-Cranfield/ros2_SimRealRobotControl.
-
--->
-
-<robot xmlns:xacro="http://ros.org/wiki/xacro">
-
-  <!-- XACRO-MACRO of Robotiq 2f-85: -->
-  <xacro:macro name="robotiq_2f85" params="bringup parent_link">
-
-    <xacro:unless value="${bringup}">  
-      
-      <!-- Robotiq 2f-85 TRANSMISSION: -->
-      <xacro:include filename="$(find ros2srrc_endeffectors)/robotiq_2f85/urdf/robotiq_2f85_transmission.xacro" />
-      <xacro:robotiq_2f85_transmission />
-
-    </xacro:unless>
-
-    <!-- ***** Robotiq 2f-85 GRIPPER .urdf SERIAL CHAIN ***** -->
-
-    <!-- Robotiq: coupler -->
-    <link name="robotiq_coupler">
-      <visual>
-        <geometry>
-          <mesh filename="file://$(find ros2srrc_endeffectors)/robotiq_2f85/meshes/robotiq_85_coupler.stl" />
-        </geometry>
-      </visual>
-      <collision>
-        <geometry>
-          <mesh filename="file://$(find ros2srrc_endeffectors)/robotiq_2f85/meshes/robotiq_85_coupler.stl" />
-        </geometry>
-      </collision>
-      <inertial>
-        <origin xyz="2.073e-05   1.45286e-03  -1.1049e-03" rpy="0 0 0" />
-        <mass value="0.168" />
-        <inertia ixx="6.69695624e-05" ixy="5.21511788e-09" ixz="-2.74383009e-08" 
-                iyy="7.85088161e-05" iyz="5.41105193e-07" izz="1.41819717e-04"/>
-      </inertial>
-    </link>
-
-    <joint name="robotiq_coupler_joint" type="fixed">
-      <origin xyz="0 0 0.004" rpy="0 0 ${-pi/2.0}" />
-      <parent link="${parent_link}"/>
-      <child link="robotiq_coupler"/>
+    <joint name="${prefix}robotiq_85_base_joint" type="fixed">
+      <parent link="${parent}"/>
+      <child link="${prefix}robotiq_85_base_link"/>
+      <xacro:insert_block name="origin"/>
     </joint>
 
-    <gazebo reference="robotiq_coupler">
-      <mu1>0.9</mu1>
-      <mu2>0.9</mu2>
-      <material>Gazebo/Black</material>
-    </gazebo>
-
-    <!-- Robotiq: base_link -->
-    <link name="robotiq_85_base_link">
+    <link name="${prefix}robotiq_85_base_link">
       <visual>
         <geometry>
           <mesh filename="file://$(find ros2srrc_endeffectors)/robotiq_2f85/meshes/visual/robotiq_85_base_link.dae"/>
@@ -88,26 +20,36 @@
       </visual>
       <collision>
         <geometry>
-          <mesh filename="file://$(find ros2srrc_endeffectors)/robotiq_2f85/meshes/collision/robotiq_85_base_link.stl"/>
+          <cylinder radius="0.04" length="0.1"/>
         </geometry>
       </collision>
       <inertial>
         <mass value="0.636951" />
         <origin xyz="0.0 0.0 0.0" />
         <inertia ixx = "0.000380" ixy = "0.000000" ixz = "0.000000"
-            iyx = "0.000000" iyy = "0.001110" iyz = "0.000000"
-            izx = "0.000000" izy = "0.000000" izz = "0.001171" />
+             iyx = "0.000000" iyy = "0.001110" iyz = "0.000000"
+             izx = "0.000000" izy = "0.000000" izz = "0.001171" />
       </inertial>
     </link>
 
-    <joint name="robotiq_85_base_joint" type="fixed">
-      <origin xyz="0 0 0.004" rpy="0 ${-pi/2} ${pi}"/>
-      <parent link="robotiq_coupler"/>
-      <child link="robotiq_85_base_link"/>
+    <joint name="${prefix}robotiq_85_left_knuckle_joint" type="revolute">
+      <parent link="${prefix}robotiq_85_base_link"/>
+      <child link="${prefix}robotiq_85_left_knuckle_link"/>
+      <axis xyz="0 0 1"/>
+      <origin rpy="${pi} 0.0 0.0" xyz="0.05490451627 0.03060114443 0.0"/>
+      <limit lower="0.0" upper="0.80285" velocity="0.5" effort="1000"/>
     </joint>
 
-    <!-- Robotiq: left_knuckle -->
-    <link name="robotiq_85_left_knuckle_link">
+    <joint name="${prefix}robotiq_85_right_knuckle_joint" type="continuous">
+      <parent link="${prefix}robotiq_85_base_link"/>
+      <child link="${prefix}robotiq_85_right_knuckle_link"/>
+      <axis xyz="0 0 1"/>
+      <origin rpy="0.0 0.0 0.0" xyz="0.05490451627 -0.03060114443 0.0"/>
+      <limit lower="-3.14" upper="3.14" velocity="100.0" effort="1000"/>
+      <mimic joint="${prefix}robotiq_85_left_knuckle_joint" multiplier="1.0"/>
+    </joint>
+
+    <link name="${prefix}robotiq_85_left_knuckle_link">
       <visual>
         <geometry>
           <mesh filename="file://$(find ros2srrc_endeffectors)/robotiq_2f85/meshes/visual/robotiq_85_knuckle_link.dae"/>
@@ -115,32 +57,19 @@
       </visual>
       <collision>
         <geometry>
-          <mesh filename="file://$(find ros2srrc_endeffectors)/robotiq_2f85/meshes/collision/robotiq_85_knuckle_link.stl"/>
+          <box size="0.03 0.03 0.03"/>
         </geometry>
       </collision>
       <inertial>
-        <origin xyz="0 0 0" rpy="0 0 0"/>
-        <mass value="0.05"/>
-        <inertia ixx="5.4167e-06" ixy="0" ixz="0"
-                iyy="1.6667e-05" iyz="0"
-                izz="1.8750e-05"/>
+        <mass value="0.018491" />
+        <origin xyz="0.0 0.0 0.0" />
+        <inertia ixx = "0.000009" ixy = "-0.000001" ixz = "0.000000"
+             iyx = "-0.000001" iyy = "0.000001" iyz = "0.000000"
+             izx = "0.000000" izy = "0.000000" izz = "0.000010" />
       </inertial>
-    </link>
-
-
-
-    <joint name="robotiq_85_left_knuckle_joint" type="revolute">
-      <parent link="robotiq_85_base_link"/>
-      <child link="robotiq_85_left_knuckle_link"/>
-      <axis xyz="0 0 1"/>
-      <origin rpy="${pi} 0.0 0.0" xyz="0.05490451627 0.03060114443 0.0"/>
-      <limit lower="-0.05" upper="0.80285" velocity="0.5" effort="1000"/>
-      <dynamics damping="1.0" friction="1.0"/>
-    </joint>
+    </link> 
 
-    <!-- Robotiq: right_knuckle -->
-    <!-- Robotiq: right_knuckle -->
-    <link name="robotiq_85_right_knuckle_link">
+    <link name="${prefix}robotiq_85_right_knuckle_link">
       <visual>
         <geometry>
           <mesh filename="file://$(find ros2srrc_endeffectors)/robotiq_2f85/meshes/visual/robotiq_85_knuckle_link.dae"/>
@@ -148,30 +77,31 @@
       </visual>
       <collision>
         <geometry>
-          <mesh filename="file://$(find ros2srrc_endeffectors)/robotiq_2f85/meshes/collision/robotiq_85_knuckle_link.stl"/>
+          <box size="0.03 0.03 0.03"/>
         </geometry>
       </collision>
       <inertial>
-        <origin xyz="0 0 0" rpy="0 0 0"/>
-        <mass value="0.05"/>
-        <inertia ixx="5.4167e-06" ixy="0" ixz="0"
-                iyy="1.6667e-05" iyz="0"
-                izz="1.8750e-05"/>
+        <mass value="0.018491" />
+        <origin xyz="0.0 0.0 0.0" />
+        <inertia ixx = "0.000009" ixy = "-0.000001" ixz = "0.000000"
+             iyx = "-0.000001" iyy = "0.000001" iyz = "0.000000"
+             izx = "0.000000" izy = "0.000000" izz = "0.000010" />
       </inertial>
     </link>
 
+    <joint name="${prefix}robotiq_85_left_finger_joint" type="fixed">
+      <parent link="${prefix}robotiq_85_left_knuckle_link"/>
+      <child link="${prefix}robotiq_85_left_finger_link"/>
+      <origin xyz="-0.00408552455 -0.03148604435 0.0" rpy="0 0 0" />
+    </joint>
 
-    <joint name="robotiq_85_right_knuckle_joint" type="revolute">
-      <parent link="robotiq_85_base_link"/>
-      <child link="robotiq_85_right_knuckle_link"/>
-      <axis xyz="0 0 1"/>
-      <origin rpy="0 0 0" xyz="0.05490451627 -0.03060114443 0.0"/>
-      <limit lower="-0.05" upper="0.80285" velocity="0.5" effort="1000"/>
-      <dynamics damping="1.0" friction="1.0"/>
+    <joint name="${prefix}robotiq_85_right_finger_joint" type="fixed">
+      <parent link="${prefix}robotiq_85_right_knuckle_link"/>
+      <child link="${prefix}robotiq_85_right_finger_link"/>
+      <origin xyz="-0.00408552455 -0.03148604435 0.0" rpy="0 0 0" />
     </joint>
 
-    <!-- Robotiq: left_finger -->
-    <link name="robotiq_85_left_finger_link">
+    <link name="${prefix}robotiq_85_left_finger_link">
       <visual>
         <geometry>
           <mesh filename="file://$(find ros2srrc_endeffectors)/robotiq_2f85/meshes/visual/robotiq_85_finger_link.dae"/>
@@ -179,27 +109,19 @@
       </visual>
       <collision>
         <geometry>
-          <mesh filename="file://$(find ros2srrc_endeffectors)/robotiq_2f85/meshes/collision/robotiq_85_finger_link.stl"/>
+          <box size="0.03 0.03 0.03"/>
         </geometry>
       </collision>
       <inertial>
         <mass value="0.027309" />
         <origin xyz="0.0 0.0 0.0" />
         <inertia ixx = "0.000003" ixy = "-0.000002" ixz = "0.000000"
-            iyx = "-0.000002" iyy = "0.000021" iyz = "0.000000"
-            izx = "0.000000" izy = "0.000000" izz = "0.000020" />
+             iyx = "-0.000002" iyy = "0.000021" iyz = "0.000000"
+             izx = "0.000000" izy = "0.000000" izz = "0.000020" />
       </inertial>
-    </link>
-
-    <joint name="robotiq_85_left_finger_joint" type="fixed">
-      <parent link="robotiq_85_left_knuckle_link"/>
-      <child link="robotiq_85_left_finger_link"/>
-      <origin xyz="-0.00408552455 -0.03148604435 0.0" rpy="0 0 0" />
-      <dynamics damping="1.0" friction="1.0"/>
-    </joint>
+    </link> 
 
-    <!-- Robotiq: right_finger -->
-    <link name="robotiq_85_right_finger_link">
+    <link name="${prefix}robotiq_85_right_finger_link">
       <visual>
         <geometry>
           <mesh filename="file://$(find ros2srrc_endeffectors)/robotiq_2f85/meshes/visual/robotiq_85_finger_link.dae"/>
@@ -207,27 +129,37 @@
       </visual>
       <collision>
         <geometry>
-          <mesh filename="file://$(find ros2srrc_endeffectors)/robotiq_2f85/meshes/collision/robotiq_85_finger_link.stl"/>
+          <box size="0.03 0.03 0.03"/>
         </geometry>
       </collision>
       <inertial>
         <mass value="0.027309" />
         <origin xyz="0.0 0.0 0.0" />
         <inertia ixx = "0.000003" ixy = "-0.000002" ixz = "0.000000"
-            iyx = "-0.000002" iyy = "0.000021" iyz = "0.000000"
-            izx = "0.000000" izy = "0.000000" izz = "0.000020" />
+             iyx = "-0.000002" iyy = "0.000021" iyz = "0.000000"
+             izx = "0.000000" izy = "0.000000" izz = "0.000020" />
       </inertial>
-    </link>
+    </link> 
 
-    <joint name="robotiq_85_right_finger_joint" type="fixed">
-      <parent link="robotiq_85_right_knuckle_link"/>
-      <child link="robotiq_85_right_finger_link"/>
-      <origin xyz="-0.00408552455 -0.03148604435 0.0" rpy="0 0 0" />
-      <dynamics damping="1.0" friction="1.0"/>
+    <joint name="${prefix}robotiq_85_left_inner_knuckle_joint" type="continuous">
+      <parent link="${prefix}robotiq_85_base_link"/>
+      <child link="${prefix}robotiq_85_left_inner_knuckle_link"/>
+      <axis xyz="0 0 1"/>
+      <origin xyz="0.06142 0.0127 0" rpy="${pi} 0.0 0.0" />
+      <limit lower="-3.14" upper="3.14" velocity="100.0" effort="0.1"/>
+      <mimic joint="${prefix}robotiq_85_left_knuckle_joint" offset="0"/>
+    </joint>
+
+    <joint name="${prefix}robotiq_85_right_inner_knuckle_joint" type="continuous">
+      <parent link="${prefix}robotiq_85_base_link"/>
+      <child link="${prefix}robotiq_85_right_inner_knuckle_link"/>
+      <axis xyz="0 0 1"/>
+      <origin xyz="0.06142 -0.0127 0" rpy="0 0 0"/>
+      <limit lower="-3.14" upper="3.14" velocity="100.0" effort="0.1"/>
+      <mimic joint="${prefix}robotiq_85_left_knuckle_joint" offset="0"/>
     </joint>
 
-    <!-- Robotiq: left_inner_knuckle -->
-    <link name="robotiq_85_left_inner_knuckle_link">
+    <link name="${prefix}robotiq_85_left_inner_knuckle_link">
       <visual>
         <geometry>
           <mesh filename="file://$(find ros2srrc_endeffectors)/robotiq_2f85/meshes/visual/robotiq_85_inner_knuckle_link.dae"/>
@@ -235,29 +167,18 @@
       </visual>
       <collision>
         <geometry>
-          <mesh filename="file://$(find ros2srrc_endeffectors)/robotiq_2f85/meshes/collision/robotiq_85_inner_knuckle_link.stl"/>
+          <box size="0.03 0.03 0.03"/>
         </geometry>
       </collision>
       <inertial>
         <mass value="0.029951" />
         <origin xyz="0.0 0.0 0.0" />
         <inertia ixx = "0.000039" ixy = "0.000000" ixz = "0.000000"
-            iyx = "0.000000" iyy = "0.000005" iyz = "0.000000"
-            izx = "0.000000" izy = "0.000000" izz = "0.000035" />
+             iyx = "0.000000" iyy = "0.000005" iyz = "0.000000"
+             izx = "0.000000" izy = "0.000000" izz = "0.000035" />
       </inertial>
     </link>
-
-    <joint name="robotiq_85_left_inner_knuckle_joint" type="continuous">
-      <parent link="robotiq_85_base_link"/>
-      <child link="robotiq_85_left_inner_knuckle_link"/>
-      <axis xyz="0 0 1"/>
-      <origin xyz="0.06142 0.0127 0" rpy="${pi} 0.0 0.0" />
-      <limit lower="-0.05" upper="0.80285" velocity="0.5" effort="1000"/>
-      <dynamics damping="1.0" friction="1.0"/>
-    </joint>
-
-    <!-- Robotiq: right_inner_knuckle -->
-    <link name="robotiq_85_right_inner_knuckle_link">
+    <link name="${prefix}robotiq_85_right_inner_knuckle_link">
       <visual>
         <geometry>
           <mesh filename="file://$(find ros2srrc_endeffectors)/robotiq_2f85/meshes/visual/robotiq_85_inner_knuckle_link.dae"/>
@@ -265,121 +186,104 @@
       </visual>
       <collision>
         <geometry>
-          <mesh filename="file://$(find ros2srrc_endeffectors)/robotiq_2f85/meshes/collision/robotiq_85_inner_knuckle_link.stl"/>
+          <box size="0.03 0.03 0.03"/>
         </geometry>
       </collision>
       <inertial>
         <mass value="0.029951" />
         <origin xyz="0.0 0.0 0.0" />
         <inertia ixx = "0.000039" ixy = "0.000000" ixz = "0.000000"
-            iyx = "0.000000" iyy = "0.000005" iyz = "0.000000"
-            izx = "0.000000" izy = "0.000000" izz = "0.000035" />
+             iyx = "0.000000" iyy = "0.000005" iyz = "0.000000"
+             izx = "0.000000" izy = "0.000000" izz = "0.000035" />
       </inertial>
     </link>
 
-    <joint name="robotiq_85_right_inner_knuckle_joint" type="continuous">
-      <parent link="robotiq_85_base_link"/>
-      <child link="robotiq_85_right_inner_knuckle_link"/>
+    <joint name="${prefix}robotiq_85_left_finger_tip_joint" type="continuous">
+      <parent link="${prefix}robotiq_85_left_inner_knuckle_link"/>
+      <child link="${prefix}robotiq_85_left_finger_tip_link"/>
       <axis xyz="0 0 1"/>
-      <origin xyz="0.06142 -0.0127 0" rpy="0 0 0"/>
-      <limit lower="-0.05" upper="0.80285" velocity="0.5" effort="1000"/>
-      <dynamics damping="1.0" friction="1.0"/>
+      <origin xyz="0.04303959807 -0.03759940821 0.0" rpy="0.0 0.0 0.0"/>
+      <limit lower="-3.14" upper="3.14" velocity="100.0" effort="0.1"/>
+      <mimic joint="${prefix}robotiq_85_left_knuckle_joint" multiplier="-1"/>
     </joint>
 
-    <!-- Robotiq: left_finger_tip -->
-    <link name="robotiq_85_left_finger_tip_link">
+    <joint name="${prefix}robotiq_85_right_finger_tip_joint" type="continuous">
+      <parent link="${prefix}robotiq_85_right_inner_knuckle_link"/>
+      <child link="${prefix}robotiq_85_right_finger_tip_link"/>
+      <axis xyz="0 0 1"/>
+      <origin rpy="0.0 0.0 0.0" xyz="0.04303959807 -0.03759940821  0.0"/>
+      <limit lower="-3.14" upper="3.14" velocity="100.0" effort="0.1"/>
+      <mimic joint="${prefix}robotiq_85_left_knuckle_joint" multiplier="-1"/>
+    </joint>
+
+
+    <link name="${prefix}robotiq_85_left_finger_tip_link">
       <visual>
         <geometry>
           <mesh filename="file://$(find ros2srrc_endeffectors)/robotiq_2f85/meshes/visual/robotiq_85_finger_tip_link.dae"/>
         </geometry>
       </visual>
       <collision>
+        <origin xyz="0.03 0 0" rpy="0 0 0"/>
         <geometry>
-          <!-- DMM: se sustituye por una caja muy pequeña para evitar que las piezas salgan disparadas -->
-          <box size= "0.005 0.005 0.005" />
-          <!--<mesh filename="file://$(find ros2srrc_endeffectors)/robotiq_2f85/meshes/collision/robotiq_85_finger_tip_link.stl"/>-->
+          <box size="0.02 0.02 0.02"/>
         </geometry>
       </collision>
       <inertial>
         <mass value="0.019555" />
         <origin xyz="0.0 0.0 0.0" />
         <inertia ixx = "0.000002" ixy = "0.000000" ixz = "0.000000"
-            iyx = "0.000000" iyy = "0.000005" iyz = "0.000000"
-            izx = "0.000000" izy = "0.000000" izz = "0.000006" />
+             iyx = "0.000000" iyy = "0.000005" iyz = "0.000000"
+             izx = "0.000000" izy = "0.000000" izz = "0.000006" />
       </inertial>
     </link> 
 
-    <joint name="robotiq_85_left_finger_tip_joint" type="continuous">
-      <parent link="robotiq_85_left_inner_knuckle_link"/>
-      <child link="robotiq_85_left_finger_tip_link"/>
-      <axis xyz="0 0 1"/>
-      <origin xyz="0.04303959807 -0.03759940821 0.0" rpy="0.0 0.0 0.0"/>
-      <limit lower="-0.80285" upper="0.05" velocity="0.5" effort="1000"/>
-      <dynamics damping="1.0" friction="1.0"/>
-    </joint>
+    <gazebo reference="${prefix}robotiq_85_left_finger_tip_link">
+      <sensor name="left_finger_contact" type="contact">
+        <always_on>true</always_on>
+        <update_rate>100</update_rate>
+        <contact>
+          <collision>
+            robotiq_85_left_finger_tip_link_collision
+          </collision>
+        </contact>
+      </sensor>
+    </gazebo>
 
-    <!-- Robotiq: right_finger_tip -->
-    <link name="robotiq_85_right_finger_tip_link">
+    <link name="${prefix}robotiq_85_right_finger_tip_link">
       <visual>
         <geometry>
           <mesh filename="file://$(find ros2srrc_endeffectors)/robotiq_2f85/meshes/visual/robotiq_85_finger_tip_link.dae"/>
         </geometry>
       </visual>
       <collision>
+        <origin xyz="0.03 0 0" rpy="0 0 0"/>
         <geometry>
-          <!-- DMM: se sustituye por una caja muy pequeña para evitar que las piezas salgan disparadas -->
-          <box size= "0.005 0.005 0.005" />
-          <!--<mesh filename="file://$(find ros2srrc_endeffectors)/robotiq_2f85/meshes/collision/robotiq_85_finger_tip_link.stl"/>-->
+          <box size="0.02 0.02 0.02"/>
         </geometry>
       </collision>
       <inertial>
         <mass value="0.019555" />
         <origin xyz="0.0 0.0 0.0" />
         <inertia ixx = "0.000002" ixy = "0.000000" ixz = "0.000000"
-            iyx = "0.000000" iyy = "0.000005" iyz = "0.000000"
-            izx = "0.000000" izy = "0.000000" izz = "0.000006" />
-      </inertial>
-    </link>
-
-    <joint name="robotiq_85_right_finger_tip_joint" type="continuous">
-      <parent link="robotiq_85_right_inner_knuckle_link"/>
-      <child link="robotiq_85_right_finger_tip_link"/>
-      <axis xyz="0 0 1"/>
-      <origin rpy="0.0 0.0 0.0" xyz="0.04303959807 -0.03759940821  0.0"/>
-      <limit lower="-0.80285" upper="0.05" velocity="0.5" effort="1000"/>
-      <dynamics damping="1.0" friction="1.0"/>
-    </joint>
-
-    <!-- SMALL CUBE FOR GRIPPER PLUGIN -->
-
-    <link name="EE_robotiq_2f85">
-      <inertial>
-        <origin xyz="0 0 0" rpy="0 0 0" />
-        <mass value="0.00001" />
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+             iyx = "0.000000" iyy = "0.000005" iyz = "0.000000"
+             izx = "0.000000" izy = "0.000000" izz = "0.000006" />
       </inertial>
-      <visual>
-        <geometry>
-          <box size= "0.001 0.001 0.001" />
-        </geometry>
-      </visual>
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0"/>
-        <geometry>
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-      </collision>
     </link>
 
-    <joint name="EE_robotiq_2f85_joint" type="fixed" >
-      <parent link="robotiq_85_base_link" />
-      <child link="EE_robotiq_2f85" />
-      <origin xyz="0.128 0.0 0.0" rpy="0 0 0"/>
-    </joint>
-
-    <gazebo reference="EE_robotiq_2f85_joint">
-      <preserveFixedJoint>true</preserveFixedJoint>
+    <gazebo reference="${prefix}robotiq_85_right_finger_tip_link">
+      <sensor name="right_finger_contact" type="contact">
+        <always_on>true</always_on>
+        <update_rate>100</update_rate>
+        <contact>
+          <collision>
+            robotiq_85_right_finger_tip_link_collision
+          </collision>
+        </contact>
+      </sensor>
     </gazebo>
     
+    <xacro:robotiq_85_gripper_transmission prefix="${prefix}" />
+
   </xacro:macro>
-</robot>
+</robot>
\ No newline at end of file
diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_ros2control.xacro b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_ros2control.xacro
index 60d683304..32637b44a 100644
--- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_ros2control.xacro
+++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_ros2control.xacro
@@ -1,137 +1,95 @@
 <?xml version="1.0"?>
-
-<!-- 
-
-# ===================================== COPYRIGHT ===================================== #
-#                                                                                       #
-#  IFRA (Intelligent Flexible Robotics and Assembly) Group, CRANFIELD UNIVERSITY        #
-#  Created on behalf of the IFRA Group at Cranfield University, United Kingdom          #
-#  E-mail: IFRA@cranfield.ac.uk                                                         #
-#                                                                                       #
-#  Licensed under the Apache-2.0 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.                    #
-#                                                                                       #
-#  IFRA Group - Cranfield University                                                    #
-#  AUTHORS: Mikel Bueno Viso - Mikel.Bueno-Viso@cranfield.ac.uk                         #
-#           Dr. Seemal Asif  - s.asif@cranfield.ac.uk                                   #
-#           Prof. Phil Webb  - p.f.webb@cranfield.ac.uk                                 #
-#                                                                                       #
-#  Date: April, 2023.                                                                   #
-#                                                                                       #
-# ===================================== COPYRIGHT ===================================== #
-
-# ======= CITE OUR WORK ======= #
-# You can cite our work with the following statement:
-# IFRA-Cranfield (2023) ROS 2 Sim-to-Real Robot Control. URL: https://github.com/IFRA-Cranfield/ros2_SimRealRobotControl.
-
--->
-
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <xacro:macro name="EE_ros2control" params="" >
+  <!-- ros2_control configuration for Robotiq 85 Gripper -->
+  <xacro:macro name="robotiq_85_ros2_control" params="
+    name
+    prefix
+    sim_gz:=false
+    use_fake_hardware:=false">
 
-    <joint name="robotiq_85_left_knuckle_joint">
-      <command_interface name="position">
-        <param name="min">-0.05</param>
-        <param name="max">0.80285</param>
-      </command_interface>
-      <command_interface name="velocity">
-        <param name="min">0.5</param>
-        <param name="max">0.5</param>
-      </command_interface>
-      <state_interface name="position">
-        <param name="initial_value">0.0</param>
-      </state_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
+    <ros2_control name="${name}_gripper" type="system">
+      <hardware>
+        <xacro:if value="${sim_gz}">
+          <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+        </xacro:if>
+        <xacro:if value="${use_fake_hardware}">
+          <plugin>mock_components/GenericSystem</plugin>
+          <param name="fake_sensor_commands">false</param>
+          <param name="state_following_offset">0.0</param>
+        </xacro:if>
+      </hardware>
+      
+      <!-- Main actuated joint -->
+      <joint name="${prefix}robotiq_85_left_knuckle_joint">
+        <command_interface name="position">
+          <param name="min">0.0</param>
+          <param name="max">0.80285</param>
+        </command_interface>
+        <state_interface name="position">
+          <param name="initial_value">0.0</param>
+        </state_interface>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
 
-    <joint name="robotiq_85_right_knuckle_joint">
-      <command_interface name="position">
-        <param name="min">-0.05</param>
-        <param name="max">0.80285</param>
-      </command_interface>
-      <command_interface name="velocity">
-        <param name="min">0.5</param>
-        <param name="max">0.5</param>
-      </command_interface>
-      <state_interface name="position">
-        <param name="initial_value">0.0</param>
-      </state_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
+      <!-- Mimic joints - need to be included for proper state publishing -->
+      <joint name="${prefix}robotiq_85_right_knuckle_joint">
+        <param name="mimic">${prefix}robotiq_85_left_knuckle_joint</param>
+        <param name="multiplier">1.0</param>
+        <command_interface name="position">
+          <param name="min">0.0</param>
+          <param name="max">0.80285</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
 
-    <joint name="robotiq_85_left_inner_knuckle_joint">
-      <command_interface name="position">
-        <param name="min">-0.05</param>
-        <param name="max">0.80285</param>
-      </command_interface>
-      <command_interface name="velocity">
-        <param name="min">0.5</param>
-        <param name="max">0.5</param>
-      </command_interface>
-      <state_interface name="position">
-        <param name="initial_value">0.0</param>
-      </state_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
+      <joint name="${prefix}robotiq_85_left_inner_knuckle_joint">
+        <param name="mimic">${prefix}robotiq_85_left_knuckle_joint</param>
+        <param name="multiplier">1.0</param>
+        <command_interface name="position">
+          <param name="min">0.0</param>
+          <param name="max">0.80285</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+      </joint>
 
-    <joint name="robotiq_85_right_inner_knuckle_joint">
-      <command_interface name="position">
-        <param name="min">-0.05</param>
-        <param name="max">0.80285</param>
-      </command_interface>
-      <command_interface name="velocity">
-        <param name="min">0.5</param>
-        <param name="max">0.5</param>
-      </command_interface>
-      <state_interface name="position">
-        <param name="initial_value">0.0</param>
-      </state_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
+      <joint name="${prefix}robotiq_85_right_inner_knuckle_joint">
+        <param name="mimic">${prefix}robotiq_85_left_knuckle_joint</param>
+        <param name="multiplier">1.0</param>
+        <command_interface name="position">
+          <param name="min">0.0</param>
+          <param name="max">0.80285</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+      </joint>
 
-    <joint name="robotiq_85_left_finger_tip_joint">
-      <command_interface name="position">
-        <param name="min">-0.80285</param>
-        <param name="max">0.05</param>
-      </command_interface>
-      <command_interface name="velocity">
-        <param name="min">0.5</param>
-        <param name="max">0.5</param>
-      </command_interface>
-      <state_interface name="position">
-        <param name="initial_value">0.0</param>
-      </state_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
+      <joint name="${prefix}robotiq_85_left_finger_tip_joint">
+        <param name="mimic">${prefix}robotiq_85_left_knuckle_joint</param>
+        <param name="multiplier">-1.0</param>
+        <command_interface name="position">
+          <param name="min">-0.80285</param>
+          <param name="max">0.0</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+      </joint>
 
-    <joint name="robotiq_85_right_finger_tip_joint">
-      <command_interface name="position">
-        <param name="min">-0.80285</param>
-        <param name="max">0.05</param>
-      </command_interface>
-      <command_interface name="velocity">
-        <param name="min">0.5</param>
-        <param name="max">0.5</param>
-      </command_interface>
-      <state_interface name="position">
-        <param name="initial_value">0.0</param>
-      </state_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
+      <joint name="${prefix}robotiq_85_right_finger_tip_joint">
+        <param name="mimic">${prefix}robotiq_85_left_knuckle_joint</param>
+        <param name="multiplier">-1.0</param>
+        <command_interface name="position">
+          <param name="min">-0.80285</param>
+          <param name="max">0.0</param>
+        </command_interface>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+      </joint>
 
+    </ros2_control>
   </xacro:macro>
-
-</robot>
\ No newline at end of file
+  </robot>
\ No newline at end of file
diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_transmission.xacro b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_transmission.xacro
index 404658c65..20eca4250 100644
--- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_transmission.xacro
+++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/robotiq_2f85/urdf/robotiq_2f85_transmission.xacro
@@ -1,128 +1,59 @@
 <?xml version="1.0"?>
 
-<!-- 
-
-# ===================================== COPYRIGHT ===================================== #
-#                                                                                       #
-#  IFRA (Intelligent Flexible Robotics and Assembly) Group, CRANFIELD UNIVERSITY        #
-#  Created on behalf of the IFRA Group at Cranfield University, United Kingdom          #
-#  E-mail: IFRA@cranfield.ac.uk                                                         #
-#                                                                                       #
-#  Licensed under the Apache-2.0 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.                    #
-#                                                                                       #
-#  IFRA Group - Cranfield University                                                    #
-#  AUTHORS: Mikel Bueno Viso - Mikel.Bueno-Viso@cranfield.ac.uk                         #
-#           Dr. Seemal Asif  - s.asif@cranfield.ac.uk                                   #
-#           Prof. Phil Webb  - p.f.webb@cranfield.ac.uk                                 #
-#                                                                                       #
-#  Date: April, 2023.                                                                   #
-#                                                                                       #
-# ===================================== COPYRIGHT ===================================== #
-
-# ======= CITE OUR WORK ======= #
-# You can cite our work with the following statement:
-# IFRA-Cranfield (2023) ROS 2 Sim-to-Real Robot Control. URL: https://github.com/IFRA-Cranfield/ros2_SimRealRobotControl.
-
--->
-
 <robot xmlns:xacro="http://wiki.ros.org/xacro">
 
   <xacro:macro name="robotiq_2f85_transmission" params="">
 
     <transmission name="robotiq_85_left_knuckle_trans">
       <type>transmission_interface/SimpleTransmission</type>
-      <joint name="robotiq_85_left_knuckle_joint">
-        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-      </joint>
-      <actuator name="robotiq_85_left_knuckle_motor">
-        <mechanicalReduction>1</mechanicalReduction>
-      </actuator>
-    </transmission>
-
-    <transmission name="robotiq_85_right_knuckle_trans">
-      <type>transmission_interface/SimpleTransmission</type>
-      <joint name="robotiq_85_right_knuckle_joint">
-        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-      </joint>
-      <actuator name="robotiq_85_right_knuckle_motor">
-        <mechanicalReduction>1</mechanicalReduction>
-      </actuator>
-    </transmission>
-
-    <transmission name="robotiq_85_left_inner_knuckle_trans">
-      <type>transmission_interface/SimpleTransmission</type>
-      <joint name="robotiq_85_left_inner_knuckle_joint">
-        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-      </joint>
-      <actuator name="robotiq_85_left_inner_knuckle_motor">
-        <mechanicalReduction>1</mechanicalReduction>
-      </actuator>
-    </transmission>
 
-    <transmission name="robotiq_85_right_inner_knuckle_trans">
-      <type>transmission_interface/SimpleTransmission</type>
-      <joint name="robotiq_85_right_inner_knuckle_joint">
-        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <joint name="robotiq_85_left_knuckle_joint">
+        <hardwareInterface>
+          hardware_interface/PositionJointInterface
+        </hardwareInterface>
       </joint>
-      <actuator name="robotiq_85_right_inner_knuckle_motor">
-        <mechanicalReduction>1</mechanicalReduction>
-      </actuator>
-    </transmission>
 
-    <transmission name="robotiq_85_left_finger_tip_trans">
-      <type>transmission_interface/SimpleTransmission</type>
-      <joint name="robotiq_85_left_finger_tip_joint">
-        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-      </joint>
-      <actuator name="robotiq_85_left_finger_tip_motor">
+      <actuator name="robotiq_85_left_knuckle_motor">
         <mechanicalReduction>1</mechanicalReduction>
-      </actuator>
-    </transmission>
-
-    <transmission name="robotiq_85_right_finger_tip_trans">
-      <type>transmission_interface/SimpleTransmission</type>
-      <joint name="robotiq_85_right_finger_tip_joint">
         <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-      </joint>
-      <actuator name="robotiq_85_right_finger_tip_motor">
-        <mechanicalReduction>1</mechanicalReduction>
       </actuator>
     </transmission>
 
     <gazebo reference="robotiq_85_base_link">
       <selfCollide>false</selfCollide>
     </gazebo>
+
     <gazebo reference="robotiq_85_left_knuckle_link">
       <selfCollide>false</selfCollide>
     </gazebo>
+
     <gazebo reference="robotiq_85_right_knuckle_link">
       <selfCollide>false</selfCollide>
     </gazebo>
+
     <gazebo reference="robotiq_85_left_finger_link">
       <selfCollide>false</selfCollide>
     </gazebo>
+
     <gazebo reference="robotiq_85_right_finger_link">
       <selfCollide>false</selfCollide>
     </gazebo>
+
     <gazebo reference="robotiq_85_left_inner_knuckle_link">
       <selfCollide>false</selfCollide>
     </gazebo>
+
     <gazebo reference="robotiq_85_right_inner_knuckle_link">
       <selfCollide>false</selfCollide>
     </gazebo>
+
     <gazebo reference="robotiq_85_left_finger_tip_link">
       <selfCollide>false</selfCollide>
     </gazebo>
+
     <gazebo reference="robotiq_85_right_finger_tip_link">
       <selfCollide>false</selfCollide>
-    </gazebo> 
+    </gazebo>
 
   </xacro:macro>
 
diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp
index 4e72de49a..cafec7d0f 100644
--- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp
+++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp
@@ -76,6 +76,7 @@
 std::string param_ROB = "none";
 std::string param_EE = "none";
 std::string param_ENV = "none";
+std::string param_ROB_GROUP = "none";
 
 // Declaration of GLOBAL VARIABLES --> MoveIt!2 Interface:
 moveit::planning_interface::MoveGroupInterface move_group_interface_ROB;
@@ -98,13 +99,22 @@ ros2srrc_data::msg::Specs eeSPECS;
 class ros2_RobotParam : public rclcpp::Node
 {
 public:
-    ros2_RobotParam() : Node("ros2_RobotParam") 
+    ros2_RobotParam() : Node("ros2_RobotParam")
     {
         this->declare_parameter("ROB_PARAM", "none");
-        param_ROB = this->get_parameter("ROB_PARAM").get_parameter_value().get<std::string>();
-        RCLCPP_INFO(this->get_logger(), "ROB_PARAM received -> %s", param_ROB.c_str());
+        param_ROB = this->get_parameter("ROB_PARAM").as_string();
+
+        this->declare_parameter("ROB_GROUP", "none");
+        param_ROB_GROUP = this->get_parameter("ROB_GROUP").as_string();
+
+        RCLCPP_INFO(this->get_logger(),
+                    "ROB_PARAM received -> %s",
+                    param_ROB.c_str());
+
+        RCLCPP_INFO(this->get_logger(),
+                    "ROB_GROUP received -> %s",
+                    param_ROB_GROUP.c_str());
     }
-private:
 };
 
 class ros2_EEParam : public rclcpp::Node
@@ -402,7 +412,7 @@ class ActionServer : public rclcpp::Node
 
             robot_trajectory::RobotTrajectory rt(
                 move_group_interface_ROB.getRobotModel(),
-                "ur5_manipulator"
+                param_ROB_GROUP
             );
 
             moveit::core::RobotStatePtr current_state = move_group_interface_ROB.getCurrentState();
@@ -559,9 +569,8 @@ int main(int argc, char ** argv)
 
     // ROBOT
     if (param_ROB != "none"){
-        std::string group_name = "ur5_manipulator";
-
-        move_group_interface_ROB = MoveGroupInterface(node2, group_name);
+        move_group_interface_ROB =
+            MoveGroupInterface(node2, param_ROB_GROUP);
 
         move_group_interface_ROB.setMaxVelocityScalingFactor(0.5);
         move_group_interface_ROB.setMaxAccelerationScalingFactor(0.5);
@@ -572,11 +581,11 @@ int main(int argc, char ** argv)
 
         joint_model_group_ROB =
             move_group_interface_ROB.getCurrentState()
-                ->getJointModelGroup(group_name);
+                ->getJointModelGroup(param_ROB_GROUP);
 
         RCLCPP_INFO(logger,
             "MoveGroupInterface created for ROBOT group: %s",
-            group_name.c_str());
+            param_ROB_GROUP.c_str());
     }
 
     // END EFFECTOR
diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp
index cfb982b46..091d8c3d6 100644
--- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp
+++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp
@@ -46,6 +46,7 @@
 #include <moveit/trajectory_processing/iterative_time_parameterization.h>
 #include <moveit/robot_trajectory/robot_trajectory.h>
 #include <geometry_msgs/msg/pose.hpp>
+#include <sstream>
 
 // Declaration of GLOBAL VARIABLE --> MoveIt!2 Interface:
 moveit::planning_interface::MoveGroupInterface move_group_interface_ROB;
@@ -56,13 +57,38 @@ std::string param_ROB = "none";
 // Declaration of GLOBAL VARIABLE --> RES:
 std::string RES = "none";
 
+// Declaration of GLOBAL VARIABLE --> ROBOT GROUP:
+std::string param_ROB_GROUP = "none";
+
+#include <cmath>
+
+void normalizeToNearest(std::vector<double>& target,
+                        const std::vector<double>& current)
+{
+    for (size_t i = 0; i < target.size(); ++i)
+    {
+        while (target[i] - current[i] > M_PI)
+            target[i] -= 2.0 * M_PI;
+
+        while (target[i] - current[i] < -M_PI)
+            target[i] += 2.0 * M_PI;
+    }
+}
+
 // =============================================================================== //
 // MoveIt!2 -> MoveGroupInterface/Plan function:
 
 moveit::planning_interface::MoveGroupInterface::Plan plan_ROB() {
 
     moveit::planning_interface::MoveGroupInterface::Plan my_plan;
-    bool success = (move_group_interface_ROB.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+    auto code = move_group_interface_ROB.plan(my_plan);
+
+    RCLCPP_INFO(
+        rclcpp::get_logger("RobMove"),
+        "MoveIt error code = %d",
+        code.val);
+
+    bool success = (code == moveit::core::MoveItErrorCode::SUCCESS);
 
     // Execute the plan
     if (success)
@@ -94,6 +120,14 @@ class ActionServer : public rclcpp::Node
         param_ROB = this->get_parameter("ROB_PARAM").as_string();
         RCLCPP_INFO(this->get_logger(), "ROB_PARAM received -> %s", param_ROB.c_str());
 
+        this->declare_parameter("ROB_GROUP", "none");
+        param_ROB_GROUP = this->get_parameter("ROB_GROUP").as_string();
+
+        RCLCPP_INFO(
+            this->get_logger(),
+            "ROB_GROUP received -> %s",
+            param_ROB_GROUP.c_str());
+
         action_server_ = rclcpp_action::create_server<Robmove>(
             this,
             "/Robmove",
@@ -137,10 +171,24 @@ class ActionServer : public rclcpp::Node
     {
 
         // 0. INFORMATION -> Current Robot Pose:
-        auto CP_INFO = move_group_interface_ROB.getCurrentPose();
-        RCLCPP_INFO(get_logger(), "INFORMATION -> Current Robot Pose:");
-        RCLCPP_INFO(get_logger(), "POSITION -> (x: %.3f, y: %.3f, z: %.3f)", CP_INFO.pose.position.x, CP_INFO.pose.position.y, CP_INFO.pose.position.z);
-        RCLCPP_INFO(get_logger(), "ORIENTATION -> (qx: %.3f, qy: %.3f, qz: %.3f, qw: %.3f)", CP_INFO.pose.orientation.x, CP_INFO.pose.orientation.y, CP_INFO.pose.orientation.z, CP_INFO.pose.orientation.w);
+       auto CP_INFO = move_group_interface_ROB.getCurrentPose();
+
+        auto state = move_group_interface_ROB.getCurrentState();
+        state->update();
+
+        const auto& current_tf = state->getGlobalLinkTransform("tool0");
+
+        RCLCPP_INFO(get_logger(),
+            "CURRENTPOSE = %.3f %.3f %.3f",
+            CP_INFO.pose.position.x,
+            CP_INFO.pose.position.y,
+            CP_INFO.pose.position.z);
+
+        RCLCPP_INFO(get_logger(),
+            "STATE TOOL = %.3f %.3f %.3f",
+            current_tf.translation().x(),
+            current_tf.translation().y(),
+            current_tf.translation().z());
 
         // 1. OBTAIN INPUT PARAMETERS:
         const auto GOAL = goal_handle->get_goal();
@@ -163,19 +211,180 @@ class ActionServer : public rclcpp::Node
         TARGET_POSE.orientation.z = GOAL->qz;
         TARGET_POSE.orientation.w = GOAL->qw;
 
-        move_group_interface_ROB.setPoseTarget(TARGET_POSE);
+        RCLCPP_INFO(
+            get_logger(),
+            "TARGET POSE = %.3f %.3f %.3f",
+            TARGET_POSE.position.x,
+            TARGET_POSE.position.y,
+            TARGET_POSE.position.z);
 
-        move_group_interface_ROB.setStartStateToCurrentState(); 
+        move_group_interface_ROB.clearPoseTargets();
+        move_group_interface_ROB.clearPathConstraints();
+
+        const moveit::core::JointModelGroup* jmg =
+            state->getJointModelGroup(param_ROB_GROUP);
+
+        bool found_ik = state->setFromIK(jmg, TARGET_POSE);
+
+        state->update();
+
+        const auto& ik_tf = state->getGlobalLinkTransform("tool0");
+
+        RCLCPP_INFO(
+            get_logger(),
+            "IK TOOL = %.3f %.3f %.3f",
+            ik_tf.translation().x(),
+            ik_tf.translation().y(),
+            ik_tf.translation().z());
+
+        RCLCPP_INFO(get_logger(), "IK FOUND = %d", found_ik);
+
+        if (!found_ik)
+        {
+            RCLCPP_ERROR(get_logger(), "NO IK SOLUTION");
+        }
+
+        if (found_ik)
+        {
+            std::vector<double> vals;
+            state->copyJointGroupPositions(jmg, vals);
+
+            // Estado actual del robot
+            std::vector<double> current =
+                move_group_interface_ROB.getCurrentJointValues();
+
+            RCLCPP_INFO(get_logger(), "CURRENT JOINTS:");
+            for (size_t i = 0; i < current.size(); ++i)
+            {
+                RCLCPP_INFO(get_logger(),
+                    "CURRENT[%zu] = %.6f",
+                    i,
+                    current[i]);
+            }
+
+            RCLCPP_INFO(get_logger(), "IK BEFORE NORMALIZATION:");
+            for (size_t i = 0; i < vals.size(); ++i)
+            {
+                RCLCPP_INFO(get_logger(),
+                    "IK[%zu] = %.6f",
+                    i,
+                    vals[i]);
+            }
+
+            // -------- NORMALIZACIÓN --------
+            normalizeToNearest(vals, current);
+
+            RCLCPP_INFO(get_logger(), "IK AFTER NORMALIZATION:");
+            for (size_t i = 0; i < vals.size(); ++i)
+            {
+                RCLCPP_INFO(get_logger(),
+                    "IKN[%zu] = %.6f",
+                    i,
+                    vals[i]);
+            }
+
+            // Pasar esos joints al planificador
+            bool ok = move_group_interface_ROB.setJointValueTarget(vals);
+
+            RCLCPP_INFO(get_logger(),
+                "setJointValueTarget = %d",
+                ok);
+
+            std::vector<double> target;
+            move_group_interface_ROB.getJointValueTarget(target);
+
+            RCLCPP_INFO(get_logger(), "TARGET JOINTS:");
+            for (size_t i = 0; i < target.size(); ++i)
+            {
+                RCLCPP_INFO(get_logger(),
+                    "TARGET[%zu] = %.6f",
+                    i,
+                    target[i]);
+            }
+        }
+        
+        state->update();
+
+        const auto &tf = state->getGlobalLinkTransform("tool0");
+
+        RCLCPP_INFO(
+            get_logger(),
+            "IK TOOL = %.3f %.3f %.3f",
+            tf.translation().x(),
+            tf.translation().y(),
+            tf.translation().z());
+
+        //move_group_interface_ROB.setPoseTarget(TARGET_POSE);
+        move_group_interface_ROB.setStartStateToCurrentState();
 
         move_group_interface_ROB.setPlannerId(GOAL->type);
         move_group_interface_ROB.setMaxVelocityScalingFactor(GOAL->speed);
 
+        // ===== DEBUG =====
+        auto joints = move_group_interface_ROB.getCurrentJointValues();
+
+        RCLCPP_INFO(get_logger(), "CURRENT JOINTS:");
+
+        for (size_t i = 0; i < joints.size(); ++i)
+        {
+            RCLCPP_INFO(
+                get_logger(),
+                "CURRENT[%zu] = %.6f",
+                i,
+                joints[i]);
+        }
+
+        // Estado interno de MoveIt
+        auto planning_state = move_group_interface_ROB.getCurrentState();
+
+        std::vector<double> planning_vals;
+
+        planning_state->copyJointGroupPositions(
+            planning_state->getJointModelGroup(param_ROB_GROUP),
+            planning_vals);
+
+        RCLCPP_INFO(get_logger(), "PLANNING STATE:");
+
+        for (size_t i = 0; i < planning_vals.size(); ++i)
+        {
+            RCLCPP_INFO(
+                get_logger(),
+                "STATE[%zu] = %.6f",
+                i,
+                planning_vals[i]);
+        }
+        // ==================
+
         MyPlan = plan_ROB();
 
         if (RES == "PLANNING: OK"){
+
+            const auto& pts = MyPlan.trajectory_.joint_trajectory.points;
+            RCLCPP_INFO(get_logger(), "Trajectory has %zu points", pts.size());
+            const auto& names = MyPlan.trajectory_.joint_trajectory.joint_names;
+
+            std::stringstream ns;
+            ns << "Joint order: ";
+
+            for (const auto& n : names)
+                ns << n << " ";
+
+            RCLCPP_INFO(get_logger(), "%s", ns.str().c_str());
+
+            for (size_t p = 0; p < pts.size(); ++p)
+            {
+                std::stringstream ss;
+                ss << "Point " << p << ": ";
+
+                for (double q : pts[p].positions)
+                    ss << q << " ";
+
+                RCLCPP_INFO(get_logger(), "%s", ss.str().c_str());
+            }
+
             robot_trajectory::RobotTrajectory rt(
                 move_group_interface_ROB.getRobotModel(),
-                "ur5_manipulator"
+                param_ROB_GROUP
             );
 
             moveit::core::RobotStatePtr current_state = move_group_interface_ROB.getCurrentState();
@@ -194,7 +403,83 @@ class ActionServer : public rclcpp::Node
 
             rt.getRobotTrajectoryMsg(MyPlan.trajectory_);
 
-            bool ExecSUCCESS = (move_group_interface_ROB.execute(MyPlan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+            // ===== DEBUG TRAJECTORY =====
+
+            RCLCPP_INFO(get_logger(), "Joint names:");
+
+            for (auto &j : MyPlan.trajectory_.joint_trajectory.joint_names)
+            {
+                RCLCPP_INFO(get_logger(), "%s", j.c_str());
+            }
+
+            RCLCPP_INFO(
+                get_logger(),
+                "Trajectory has %zu points",
+                MyPlan.trajectory_.joint_trajectory.points.size());
+
+            auto &first = MyPlan.trajectory_.joint_trajectory.points.front();
+            auto &last  = MyPlan.trajectory_.joint_trajectory.points.back();
+
+            RCLCPP_INFO(get_logger(), "FIRST POINT");
+
+            for (size_t i = 0; i < first.positions.size(); ++i)
+            {
+                RCLCPP_INFO(
+                    get_logger(),
+                    "%s = %.6f",
+                    MyPlan.trajectory_.joint_trajectory.joint_names[i].c_str(),
+                    first.positions[i]);
+            }
+
+            RCLCPP_INFO(get_logger(), "LAST POINT");
+
+            for (size_t i = 0; i < last.positions.size(); ++i)
+            {
+                RCLCPP_INFO(
+                    get_logger(),
+                    "%s = %.6f",
+                    MyPlan.trajectory_.joint_trajectory.joint_names[i].c_str(),
+                    last.positions[i]);
+            }
+
+            // ============================
+
+            bool ExecSUCCESS =
+                (move_group_interface_ROB.execute(MyPlan)
+                == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+
+            auto state = move_group_interface_ROB.getCurrentState();
+
+            std::vector<double> q;
+            state->copyJointGroupPositions(
+                state->getJointModelGroup(param_ROB_GROUP),
+                q);
+
+            RCLCPP_INFO(get_logger(), "ROBOT AFTER EXECUTION");
+
+            for(size_t i=0;i<q.size();i++)
+            {
+                RCLCPP_INFO(get_logger(),
+                    "Q[%zu]=%.6f",
+                    i,
+                    q[i]);
+            }
+
+            move_group_interface_ROB.clearPoseTargets();
+            move_group_interface_ROB.clearPathConstraints();
+
+            auto end_joints = move_group_interface_ROB.getCurrentJointValues();
+
+            RCLCPP_INFO(get_logger(), "FINAL JOINTS:");
+
+            for (size_t i = 0; i < end_joints.size(); ++i)
+            {
+                RCLCPP_INFO(
+                    get_logger(),
+                    "FINAL[%zu] = %.6f",
+                    i,
+                    end_joints[i]);
+            }
 
             if (goal_handle->is_canceling()) {
                 RCLCPP_INFO(this->get_logger(), "ROBOT MOVEMENT (%s) has been CANCELED.", GOAL->type.c_str());
@@ -271,14 +556,16 @@ int main(int argc, char **argv)
 
     using moveit::planning_interface::MoveGroupInterface;
 
-    std::string ROBname = "ur5_manipulator";
-
-    move_group_interface_ROB = MoveGroupInterface(moveit_node, ROBname);
+    move_group_interface_ROB =
+    MoveGroupInterface(moveit_node, param_ROB_GROUP);
 
     move_group_interface_ROB.setMaxVelocityScalingFactor(1.0);
     move_group_interface_ROB.setMaxAccelerationScalingFactor(1.0);
 
-    RCLCPP_INFO(logger, "MoveGroupInterface object created for ROBOT: %s", ROBname.c_str());
+    RCLCPP_INFO(
+        logger,
+        "MoveGroupInterface object created for ROBOT: %s",
+        param_ROB_GROUP.c_str());
 
     rclcpp::spin(node);
 
diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp
index 524a1e498..fc7b462d5 100644
--- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp
+++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robpose.cpp
@@ -53,6 +53,8 @@ moveit::planning_interface::MoveGroupInterface move_group_interface_ROB;
 // Declaration of GLOBAL VARIABLE --> ROBOT PARAMETER:
 std::string param_ROB = "none";
 
+std::string param_ROB_GROUP = "none";
+
 // Declaration of GLOBAL VARIABLE --> ROBOT POSE:
 ros2srrc_data::msg::Robpose POSE; 
 
@@ -113,11 +115,14 @@ int main(int argc, char **argv)
 
   RCLCPP_INFO(node->get_logger(), "ROB_PARAM received -> %s", param_ROB.c_str());
 
+  node->declare_parameter("ROB_GROUP", "none");
+  param_ROB_GROUP =
+      node->get_parameter("ROB_GROUP").as_string();
+
   // === MOVEIT ===
   using moveit::planning_interface::MoveGroupInterface;
-  std::string ROBname = "ur5_manipulator";
-
-  move_group_interface_ROB = MoveGroupInterface(node, ROBname);
+  move_group_interface_ROB =
+    MoveGroupInterface(node, param_ROB_GROUP);
 
   RCLCPP_INFO(node->get_logger(), "MoveGroupInterface created");
 
diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/controller.yaml b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/controller.yaml
index d5b1ac25c..26180e975 100644
--- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/controller.yaml
+++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/controller.yaml
@@ -1,32 +1,96 @@
 controller_manager:
   ros__parameters:
-    
-    update_rate: 250 #Hz
+
+    update_rate: 100
 
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
-    
+
     joint_trajectory_controller:
       type: joint_trajectory_controller/JointTrajectoryController
 
+    gripper_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+
+joint_state_broadcaster:
+  ros__parameters:
+
+    joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+      - robotiq_85_left_knuckle_joint
+
+    interfaces:
+      - position
+      - velocity
+      - effort
+
+
 joint_trajectory_controller:
   ros__parameters:
+
     joints:
-        - shoulder_pan_joint
-        - shoulder_lift_joint
-        - elbow_joint
-        - wrist_1_joint
-        - wrist_2_joint
-        - wrist_3_joint
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+
     command_interfaces:
       - position
+
+    state_interfaces:
+      - position
       - velocity
+
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+
+    allow_partial_joints_goal: false
+    allow_nonzero_velocity_at_trajectory_end: true
+    allow_integration_in_goal_trajectories: true
+
+    constraints:
+      stopped_velocity_tolerance: 0.01
+      goal_time: 0.0
+
+
+gripper_controller:
+  ros__parameters:
+
+    joints:
+      - robotiq_85_left_knuckle_joint
+
+    command_interfaces:
+      - position
+
     state_interfaces:
       - position
       - velocity
-    state_publish_rate: 250.0
+
+    state_publish_rate: 100.0
     action_monitor_rate: 20.0
+
     allow_partial_joints_goal: false
+    allow_integration_in_goal_trajectories: true
+
     constraints:
-      stopped_velocity_tolerance: 0.0
-      goal_time: 0.0
\ No newline at end of file
+      stopped_velocity_tolerance: 0.01
+      goal_time: 0.0
+
+      robotiq_85_left_knuckle_joint:
+        trajectory: 0.5
+        goal: 0.5
+
+    gains:
+      robotiq_85_left_knuckle_joint:
+        p: 100.0
+        d: 1.0
+        i: 0.01
+        i_clamp: 1.0
\ No newline at end of file
diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/moveit_controllers.yaml b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/moveit_controllers.yaml
new file mode 100644
index 000000000..ef495fd5f
--- /dev/null
+++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/moveit_controllers.yaml
@@ -0,0 +1,20 @@
+/**:
+  ros__parameters:
+
+    moveit_simple_controller_manager:
+
+      controller_names:
+        - joint_trajectory_controller
+
+      joint_trajectory_controller:
+        type: FollowJointTrajectory
+        action_ns: follow_joint_trajectory
+        default: true
+
+        joints:
+          - shoulder_pan_joint
+          - shoulder_lift_joint
+          - elbow_joint
+          - wrist_1_joint
+          - wrist_2_joint
+          - wrist_3_joint
\ No newline at end of file
diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/ompl_planning.yaml b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/ompl_planning.yaml
new file mode 100644
index 000000000..24eabd179
--- /dev/null
+++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/config/ompl_planning.yaml
@@ -0,0 +1,67 @@
+/**:
+  ros__parameters:
+
+    planner_configs:
+
+      SBLkConfigDefault:
+        type: geometric::SBL
+
+      ESTkConfigDefault:
+        type: geometric::EST
+        goal_bias: 0.05
+
+      LBKPIECEkConfigDefault:
+        type: geometric::LBKPIECE
+        border_fraction: 0.9
+        min_valid_path_fraction: 0.5
+
+      BKPIECEkConfigDefault:
+        type: geometric::BKPIECE
+        border_fraction: 0.9
+        failed_expansion_score_factor: 0.5
+        min_valid_path_fraction: 0.5
+
+      KPIECEkConfigDefault:
+        type: geometric::KPIECE
+        goal_bias: 0.05
+        border_fraction: 0.9
+        failed_expansion_score_factor: 0.5
+        min_valid_path_fraction: 0.5
+
+      RRTkConfigDefault:
+        type: geometric::RRT
+        goal_bias: 0.05
+
+      RRTConnectkConfigDefault:
+        type: geometric::RRTConnect
+
+      RRTstarkConfigDefault:
+        type: geometric::RRTstar
+        goal_bias: 0.05
+        delay_collision_checking: 1
+
+      TRRTkConfigDefault:
+        type: geometric::TRRT
+        goal_bias: 0.05
+
+      PRMkConfigDefault:
+        type: geometric::PRM
+
+      PRMstarkConfigDefault:
+        type: geometric::PRMstar
+
+    ur3_arm:
+      planner_configs:
+        - SBLkConfigDefault
+        - ESTkConfigDefault
+        - LBKPIECEkConfigDefault
+        - BKPIECEkConfigDefault
+        - KPIECEkConfigDefault
+        - RRTkConfigDefault
+        - RRTConnectkConfigDefault
+        - RRTstarkConfigDefault
+        - TRRTkConfigDefault
+        - PRMkConfigDefault
+        - PRMstarkConfigDefault
+
+      longest_valid_segment_fraction: 0.01
\ No newline at end of file
diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/urdf/ur3_macro.urdf.xacro b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/urdf/ur3_macro.urdf.xacro
index 9fafebecc..2219b142a 100644
--- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/urdf/ur3_macro.urdf.xacro
+++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/urdf/ur3_macro.urdf.xacro
@@ -99,9 +99,6 @@
         <geometry>
           <mesh filename="file://$(find ros2srrc_robots)/ur3/meshes/visual/base.dae"/>
         </geometry>
-        <material name="LightGrey">
-          <color rgba="0.7 0.7 0.7 1.0"/>
-        </material>
       </visual>
       <collision>
         <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
@@ -119,9 +116,6 @@
         <geometry>
           <mesh filename="file://$(find ros2srrc_robots)/ur3/meshes/visual/shoulder.dae"/>
         </geometry>
-        <material name="LightGrey">
-          <color rgba="0.7 0.7 0.7 1.0"/>
-        </material>
       </visual>
       <collision>
         <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
@@ -139,9 +133,6 @@
         <geometry>
           <mesh filename="file://$(find ros2srrc_robots)/ur3/meshes/visual/upperarm.dae"/>
         </geometry>
-        <material name="LightGrey">
-          <color rgba="0.7 0.7 0.7 1.0"/>
-        </material>
       </visual>
       <collision>
         <origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
@@ -159,9 +150,6 @@
         <geometry>
           <mesh filename="file://$(find ros2srrc_robots)/ur3/meshes/visual/forearm.dae"/>
         </geometry>
-        <material name="LightGrey">
-          <color rgba="0.7 0.7 0.7 1.0"/>
-        </material>
       </visual>
       <collision>
         <origin xyz="0 0 ${elbow_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
@@ -179,9 +167,6 @@
         <geometry>
           <mesh filename="file://$(find ros2srrc_robots)/ur3/meshes/visual/wrist1.dae"/>
         </geometry>
-        <material name="LightGrey">
-          <color rgba="0.7 0.7 0.7 1.0"/>
-        </material>
       </visual>
       <collision>
         <origin xyz="0 0 ${wrist_1_visual_offset}" rpy="${pi/2} 0 0"/>
@@ -199,9 +184,6 @@
         <geometry>
           <mesh filename="file://$(find ros2srrc_robots)/ur3/meshes/visual/wrist2.dae"/>
         </geometry>
-        <material name="LightGrey">
-          <color rgba="0.7 0.7 0.7 1.0"/>
-        </material>
       </visual>
       <collision>
         <origin xyz="0 0 ${wrist_2_visual_offset}" rpy="0 0 0"/>
@@ -219,9 +201,6 @@
         <geometry>
           <mesh filename="file://$(find ros2srrc_robots)/ur3/meshes/visual/wrist3.dae"/>
         </geometry>
-        <material name="LightGrey">
-          <color rgba="0.7 0.7 0.7 1.0"/>
-        </material>
       </visual>
       <collision>
         <origin xyz="0 0 ${wrist_3_visual_offset}" rpy="${pi/2} 0 0"/>
diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/urdf/ur3_ros2control.xacro b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/urdf/ur3_ros2control.xacro
index a317468c0..3900a0aff 100644
--- a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/urdf/ur3_ros2control.xacro
+++ b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur3/urdf/ur3_ros2control.xacro
@@ -144,7 +144,7 @@
       <!-- A. GAZEBO SIMULATION: -->
       <xacro:unless value="${bringup}">
         <hardware>
-          <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+          <plugin>gz_ros2_control/GazeboSimSystem</plugin>
         </hardware>
       </xacro:unless>
 
diff --git a/Industrial/ur3_gripper_moveit_config /CMakeLists.txt b/Industrial/ur3_gripper_moveit_config /CMakeLists.txt
new file mode 100644
index 000000000..e2513fb58
--- /dev/null
+++ b/Industrial/ur3_gripper_moveit_config /CMakeLists.txt	
@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.8)
+project(ur3_gripper_moveit_config)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(robotiq_description REQUIRED)
+find_package(urdf REQUIRED)
+find_package(xacro REQUIRED)
+
+install(DIRECTORY config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/Industrial/ur3_gripper_moveit_config /config/controllers.yaml b/Industrial/ur3_gripper_moveit_config /config/controllers.yaml
new file mode 100644
index 000000000..568d22a69
--- /dev/null
+++ b/Industrial/ur3_gripper_moveit_config /config/controllers.yaml	
@@ -0,0 +1,26 @@
+/**:
+  ros__parameters:
+    moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
+    moveit_simple_controller_manager:
+      controller_names:
+        - joint_trajectory_controller
+        - gripper_controller
+
+      joint_trajectory_controller:
+        action_ns: follow_joint_trajectory
+        type: FollowJointTrajectory
+        default: true
+        joints:
+          - shoulder_pan_joint
+          - shoulder_lift_joint
+          - elbow_joint
+          - wrist_1_joint
+          - wrist_2_joint
+          - wrist_3_joint
+      
+      gripper_controller:
+        action_ns: follow_joint_trajectory
+        type: FollowJointTrajectory
+        default: false
+        joints:
+          - robotiq_85_left_knuckle_joint
diff --git a/Industrial/ur3_gripper_moveit_config /config/kinematics.yaml b/Industrial/ur3_gripper_moveit_config /config/kinematics.yaml
new file mode 100644
index 000000000..16412cd0b
--- /dev/null
+++ b/Industrial/ur3_gripper_moveit_config /config/kinematics.yaml	
@@ -0,0 +1,9 @@
+/**:
+  ros__parameters:
+    ur3_arm:
+      kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+      kinematics_solver_search_resolution: 0.005
+      kinematics_solver_timeout: 0.1
+      kinematics_solver_attempts: 10
+    gripper:
+      kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
\ No newline at end of file
diff --git a/Industrial/ur3_gripper_moveit_config /config/moveit_controllers.yaml b/Industrial/ur3_gripper_moveit_config /config/moveit_controllers.yaml
new file mode 100644
index 000000000..188c35ddd
--- /dev/null
+++ b/Industrial/ur3_gripper_moveit_config /config/moveit_controllers.yaml	
@@ -0,0 +1,25 @@
+/**:
+  ros__parameters:
+    moveit_simple_controller_manager:
+      controller_names:
+        - joint_trajectory_controller
+        - gripper_controller
+
+      joint_trajectory_controller:
+        type: FollowJointTrajectory
+        action_ns: follow_joint_trajectory
+        default: true
+        joints:
+          - shoulder_pan_joint
+          - shoulder_lift_joint
+          - elbow_joint
+          - wrist_1_joint
+          - wrist_2_joint
+          - wrist_3_joint
+
+      gripper_controller:
+        type: FollowJointTrajectory
+        action_ns: follow_joint_trajectory
+        default: true
+        joints:
+          - robotiq_85_left_knuckle_joint
\ No newline at end of file
diff --git a/Industrial/ur3_gripper_moveit_config /config/ompl_planning.yaml b/Industrial/ur3_gripper_moveit_config /config/ompl_planning.yaml
new file mode 100644
index 000000000..1fae3f8d6
--- /dev/null
+++ b/Industrial/ur3_gripper_moveit_config /config/ompl_planning.yaml	
@@ -0,0 +1,72 @@
+/**:
+  ros__parameters:
+    planner_configs:
+      SBLkConfigDefault:
+        type: geometric::SBL
+        range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+      ESTkConfigDefault:
+        type: geometric::EST
+        range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
+        goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+      LBKPIECEkConfigDefault:
+        type: geometric::LBKPIECE
+        range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+        border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
+        min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+      BKPIECEkConfigDefault:
+        type: geometric::BKPIECE
+        range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+        border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
+        failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
+        min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+      KPIECEkConfigDefault:
+        type: geometric::KPIECE
+        range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+        goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+        border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9 (0.0,1.]
+        failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
+        min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+      RRTkConfigDefault:
+        type: geometric::RRT
+        range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+        goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+      RRTConnectkConfigDefault:
+        type: geometric::RRTConnect
+        range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+      RRTstarkConfigDefault:
+        type: geometric::RRTstar
+        range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+        goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+        delay_collision_checking: 1  # Stop collision checking as soon as C-free parent found. default 1
+      TRRTkConfigDefault:
+        type: geometric::TRRT
+        range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+        goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+        max_states_failed: 10  # when to start increasing temp. default: 10
+        temp_change_factor: 2.0  # how much to increase or decrease temp. default: 2.0
+        min_temperature: 10e-10  # lower limit of temp change. default: 10e-10
+        init_temperature: 10e-6  # initial temperature. default: 10e-6
+        frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
+        frountierNodeRatio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
+        k_constant: 0.0  # value used to normalize expression. default: 0.0 set in setup()
+      PRMkConfigDefault:
+        type: geometric::PRM
+        max_nearest_neighbors: 10  # use k nearest neighbors. default: 10
+      PRMstarkConfigDefault:
+        type: geometric::PRMstar
+    ur3_arm:
+      planner_configs:
+        - SBLkConfigDefault
+        - ESTkConfigDefault
+        - LBKPIECEkConfigDefault
+        - BKPIECEkConfigDefault
+        - KPIECEkConfigDefault
+        - RRTkConfigDefault
+        - RRTConnectkConfigDefault
+        - RRTstarkConfigDefault
+        - TRRTkConfigDefault
+        - PRMkConfigDefault
+        - PRMstarkConfigDefault
+      ##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE
+      #projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint)
+      longest_valid_segment_fraction: 0.01
diff --git a/Industrial/ur3_gripper_moveit_config /config/ur_servo.yaml b/Industrial/ur3_gripper_moveit_config /config/ur_servo.yaml
new file mode 100644
index 000000000..741c97b6a
--- /dev/null
+++ b/Industrial/ur3_gripper_moveit_config /config/ur_servo.yaml	
@@ -0,0 +1,76 @@
+###############################################
+# Modify all parameters related to servoing here
+###############################################
+use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
+
+## Properties of incoming commands
+command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
+scale:
+  # Scale parameters are only used if command_in_type=="unitless"
+  linear:  0.6  # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
+  rotational:  0.3 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
+  # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic.
+  joint: 0.01
+# This is a fudge factor to account for any latency in the system, e.g. network latency or poor low-level
+# controller performance. It essentially increases the timestep when calculating the target pose, to move the target
+# pose farther away. [seconds]
+system_latency_compensation: 0.04
+
+## Properties of outgoing commands
+publish_period: 0.004  # 1/Nominal publish rate [seconds]
+low_latency_mode: false  # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
+
+# What type of topic does your robot driver expect?
+# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
+command_out_type: std_msgs/Float64MultiArray
+
+# What to publish? Can save some bandwidth as most robots only require positions or velocities
+publish_joint_positions: true
+publish_joint_velocities: false
+publish_joint_accelerations: false
+
+## Plugins for smoothing outgoing commands
+smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
+
+## Incoming Joint State properties
+low_pass_filter_coeff: 10.0  # Larger --> trust the filtered data more, trust the measurements less.
+
+## MoveIt properties
+move_group_name:  ur3_arm # Often 'manipulator' or 'arm'
+planning_frame: base_link  # The MoveIt planning frame. Often 'base_link' or 'world'
+
+## Other frames
+ee_frame_name: tool0  # The name of the end effector link, used to return the EE pose
+robot_link_command_frame:  tool0  # commands must be given in the frame of a robot link. Usually either the base or end effector
+
+## Stopping behaviour
+incoming_command_timeout:  0.1  # Stop servoing if X seconds elapse without a new command
+# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
+# Important because ROS may drop some messages and we need the robot to halt reliably.
+num_outgoing_halt_msgs_to_publish: 4
+
+## Configure handling of singularities and joint limits
+lower_singularity_threshold:  100.0  # Start decelerating when the condition number hits this (close to singularity)
+hard_stop_singularity_threshold: 200.0 # Stop when the condition number hits this
+joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
+
+## Topic names
+cartesian_command_in_topic: ~/delta_twist_cmds  # Topic for incoming Cartesian twist commands
+joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands
+joint_topic: /joint_states
+status_topic: ~/status # Publish status to this topic
+command_out_topic: /forward_position_controller/commands # Publish outgoing commands here
+
+## Collision checking for the entire robot body
+check_collisions: true # Check collisions?
+collision_check_rate: 5.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
+# Two collision check algorithms are available:
+# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually.
+# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits
+collision_check_type: threshold_distance
+# Parameters for "threshold_distance"-type collision checking
+self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
+scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]
+# Parameters for "stop_distance"-type collision checking
+collision_distance_safety_factor: 1000.0 # Must be >= 1. A large safety factor is recommended to account for latency
+min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m]
diff --git a/Industrial/ur3_gripper_moveit_config /package.xml b/Industrial/ur3_gripper_moveit_config /package.xml
new file mode 100644
index 000000000..b5e616ab1
--- /dev/null
+++ b/Industrial/ur3_gripper_moveit_config /package.xml	
@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ur3_gripper_moveit_config</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="chenjuotung@gmail.com">justin</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+  <depend>robotiq_description</depend>
+  <depend>urdf</depend>
+  <depend>xacro</depend>
+
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>moveit_kinematics</exec_depend>
+  <exec_depend>moveit_planners_ompl</exec_depend>
+  <exec_depend>moveit_ros_move_group</exec_depend>
+  <exec_depend>moveit_ros_visualization</exec_depend>
+  <exec_depend>moveit_servo</exec_depend>
+  <exec_depend>moveit_simple_controller_manager</exec_depend>
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>joint_trajectory_controller</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/Launchers/rviz/sausage_exercise.launch.py b/Launchers/rviz/sausage_exercise.launch.py
new file mode 100644
index 000000000..aeccab830
--- /dev/null
+++ b/Launchers/rviz/sausage_exercise.launch.py
@@ -0,0 +1,142 @@
+"""
+UR3 + Robotiq - RViz Launcher
+Launches ONLY RViz with MoveIt Motion Planning
+Assumes Gazebo and robot are already running
+"""
+
+import os
+import xacro
+import yaml
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import TimerAction
+
+from ament_index_python.packages import get_package_share_directory
+
+def load_yaml(package_name, file_path):
+    pkg_path = get_package_share_directory(package_name)
+    with open(os.path.join(pkg_path, file_path), "r") as f:
+        return yaml.safe_load(f)
+
+
+def generate_launch_description():
+
+    # =====================================================
+    # Packages
+    # =====================================================
+
+    pkg_share_dir = get_package_share_directory("custom_robots")
+    moveit_pkg_share = get_package_share_directory("ros2srrc_ur3_moveit2")
+
+    # =====================================================
+    # URDF
+    # =====================================================
+
+    xacro_file = os.path.join(
+        pkg_share_dir,
+        "models",
+        "ur3",
+        "ur3.urdf.xacro",
+    )
+
+    controllers_file = os.path.join(
+        pkg_share_dir,
+        "config",
+        "ur3_controllers.yaml",
+    )
+
+    robot_description_content = xacro.process_file(
+        xacro_file,
+        mappings={
+            "ur_type": "ur3",
+            "name": "ur",
+            "prefix": "",
+            "use_fake_hardware": "false",
+            "sim_gazebo": "false",
+            "sim_gz": "true",
+            "simulation_controllers": controllers_file,
+            "hmi": "false",
+            "EE": "true",
+            "EE_name": "robotiq_2f85",
+            "camera": "false",
+        },
+    ).toxml()
+
+    robot_description = {
+        "robot_description": robot_description_content
+    }
+
+    # =====================================================
+    # SRDF
+    # =====================================================
+
+    srdf_file = os.path.join(
+        moveit_pkg_share,
+        "config",
+        "ur3robotiq_2f85.srdf",
+    )
+
+    with open(srdf_file, "r") as file:
+        robot_description_semantic = {
+            "robot_description_semantic": file.read()
+        }
+
+    # =====================================================
+    # MoveIt Config Files
+    # =====================================================
+
+    kinematics_yaml = load_yaml(
+        "ur3_gripper_moveit_config",
+        "config/kinematics.yaml",
+    )
+
+    kinematics_yaml = {
+        "robot_description_kinematics":
+            kinematics_yaml["/**"]["ros__parameters"]
+    }
+
+    ompl_planning = load_yaml(
+        "ros2srrc_robots",
+        "ur3/config/ompl_planning.yaml",
+    )
+
+    ompl_planning = ompl_planning["/**"]["ros__parameters"]
+
+    # =====================================================
+    # RViz Config
+    # =====================================================
+
+    rviz_config_file = (
+        "/home/ws/src/Industrial/ros2_SimRealRobotControl_gz/"
+        "packages/ur3/ros2srrc_ur3_moveit2/rviz/moveit.rviz"
+    )
+
+    print("MOVEIT PKG =", moveit_pkg_share)
+    print("RVIZ FILE =", rviz_config_file)
+
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config_file],
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            ompl_planning,
+            kinematics_yaml,
+            {"use_sim_time": True},
+        ],
+    )
+
+    delay_rviz = TimerAction(
+        period=3.0,
+        actions=[rviz_node],
+    )
+
+    return LaunchDescription(
+        [
+            delay_rviz,
+        ]
+    )
\ No newline at end of file
diff --git a/Launchers/sausage_exercise.launch.py b/Launchers/sausage_exercise.launch.py
index eff793839..9468f828a 100644
--- a/Launchers/sausage_exercise.launch.py
+++ b/Launchers/sausage_exercise.launch.py
@@ -6,6 +6,7 @@
 from launch.actions import ExecuteProcess, SetEnvironmentVariable
 
 from ament_index_python.packages import get_package_share_directory
+from launch_ros.actions import Node
 
 
 def generate_launch_description():
@@ -70,11 +71,20 @@ def generate_launch_description():
         output="screen",
     )
 
+    sausage_spawner = ExecuteProcess(
+        cmd=[
+            "python3",
+            "home/ws/src/CustomRobots/conveyor_belt/spawn_sausage.py"
+        ],
+        output="screen",
+    )
+    
     return LaunchDescription(
         [
             set_gz_plugin_path,
             set_ld_library_path,
             set_resource_path,
             gz,
+            sausage_spawner,
         ]
     )
diff --git a/Worlds/sausage_exercise.world b/Worlds/sausage_exercise.world
index 2528dcdf6..e90e7bc06 100644
--- a/Worlds/sausage_exercise.world
+++ b/Worlds/sausage_exercise.world
@@ -12,8 +12,10 @@
     <plugin filename="gz-sim-scene-broadcaster-system"
             name="gz::sim::systems::SceneBroadcaster"/>
 
-    <plugin filename="gz-sim-factory-system"
-            name="gz::sim::systems::Factory"/>
+    <plugin filename="gz-sim-sensors-system"
+            name="gz::sim::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
 
     <gravity>0 0 -9.8</gravity>
 
@@ -71,6 +73,36 @@
       <pose>0 0 0 0 0 0</pose>
     </include>
 
+    <!-- Robot Base (Placeholder) -->
+    <model name="robotbase">
+      <static>true</static>
+      <pose>-0.5 0 0 0 0 0</pose>
+      <link name="link">
+        <pose>0 0 0.45 0 0 0</pose>
+        <visual name="visual">
+          <geometry>
+            <cylinder>
+              <radius>0.09</radius>
+              <length>0.9</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.3 0.3 0.3 1</ambient>
+            <diffuse>0.7 0.7 0.7 1</diffuse>
+            <specular>0.01 0.01 0.01 1</specular>
+          </material>
+        </visual>
+        <collision name="collision">
+          <geometry>
+            <cylinder>
+              <radius>0.09</radius>
+              <length>0.9</length>
+            </cylinder>
+          </geometry>
+        </collision>
+      </link>
+    </model>
+
   </world>
 
 </sdf>
\ No newline at end of file
diff --git a/database/universes.sql b/database/universes.sql
index 238550680..674039810 100644
--- a/database/universes.sql
+++ b/database/universes.sql
@@ -198,7 +198,7 @@ COPY public.universes (id, name, world_id, robot_id) FROM stdin;
 68	Rover 4wd Warehouse Low Noise	68	19
 69	Rover 4wd Warehouse High Noise	68	20
 70	Machine vision Harmonic world	70	24
-71	Conveyor Belt World	71	0
+71	Conveyor Belt World	71	26
 \.
 
 --
@@ -265,7 +265,7 @@ COPY public.worlds (id, name, launch_file_path, tools_config, ros_version, type,
 67	Follow Person Teleop Harmonic	/opt/jderobot/Launchers/follow_person_teleop_harmonic.launch.py	{"gzsim":"/opt/jderobot/Launchers/visualization/follow_person.config"}	ROS2	gz	{-1.0,10.0,0.1,0.0,0.0,0.0}
 68	Rover 4wd Warehouse Low Noise	/opt/jderobot/Launchers/rover_4wd_warehouse.launch.py	None	ROS2	gz	{0.0,0.0,0.0,0.0,0.0,0.0}
 70	Machine Vision Harmonic	/opt/jderobot/Launchers/machine_vision.launch.py	{"rviz":"/opt/jderobot/Launchers/rviz/pick_place_harmonic.launch.py"}	ROS2	gz	{0.0,0.0,0.9,0.0,0.0,0.0}
-71	Conveyor Harmonic	/opt/jderobot/Launchers/sausage_exercise.launch.py	None	ROS2	gz	{0.0,0.0,0.0,0.0,0.0,0.0}
+71	Conveyor Harmonic	/opt/jderobot/Launchers/sausage_exercise.launch.py	{"rviz":"/opt/jderobot/Launchers/rviz/sausage_exercise.launch.py"}	ROS2	gz	{-0.5,0.0,0.9,0.0,0.0,0.0}
 \.
 
 --
@@ -299,6 +299,8 @@ COPY public.robots (id, name, launch_file_path, entity, extra_config) FROM stdin
 22	Turtlebot 3 Medium Noise	/home/ws/src/CustomRobots/turtlebot3/launch/turtlebot3.launch.py	turtlebot3	noise:=med
 23	Turtlebot 3 High Noise	/home/ws/src/CustomRobots/turtlebot3/launch/turtlebot3.launch.py	turtlebot3	noise:=high
 24	Ur5 Camera	/home/ws/src/CustomRobots/robot_arms/launch/ur5.launch.py	ur5_robotiq	sensor:=camera
+25	Ur3	/home/ws/src/CustomRobots/robot_arms/launch/ur3.launch.py	ur3_robotiq	None
+26	Ur3 Camera	/home/ws/src/CustomRobots/robot_arms/launch/ur3.launch.py	ur3_robotiq	sensor:=camera
 \.
 
 --