From a8088a404c585639b19bfcc0a1e9378b4c7169d2 Mon Sep 17 00:00:00 2001
From: Niko <nikolaus.feith@unileoben.ac.at>
Date: Wed, 20 Mar 2024 19:11:53 +0100
Subject: [PATCH] urdf and srdf are not passed as dicts but as
 {'robot_description': <launch.substitutions.command.Command object at
 0x7358a27fb6d0>} -> maybe the mistake

---
 .vscode/c_cpp_properties.json                 |  66 --
 .vscode/settings.json                         |  26 -
 src/Universal_Robots_ROS2_Driver              |   2 +-
 .../urdf/ur_robotiq.urdf                      | 904 ------------------
 .../urdf/ur_robotiq.urdf.xacro                | 102 +-
 src/ur_robotiq_moveit_config/.setup_assistant |  27 -
 src/ur_robotiq_moveit_config/CMakeLists.txt   |   3 +-
 .../config/controllers.yaml                   |  29 +
 .../config/initial_positions.yaml             |  10 -
 .../config/joint_limits.yaml                  |   7 +-
 .../config/kinematics.yaml                    |  16 +-
 .../config/moveit_controllers.yaml            |  26 -
 .../config/ompl_planning.yaml                 |  70 ++
 .../config/pilz_cartesian_limits.yaml         |   6 -
 .../config/ros2_controllers.yaml              |  41 -
 .../config/ur3e_robotiq.ros2_control.xacro    |  70 --
 .../config/ur3e_robotiq.srdf                  | 164 ----
 .../config/ur3e_robotiq.urdf.xacro            |  14 -
 .../config/ur_servo.yaml                      |  76 ++
 .../launch/demo.launch.py                     |   4 +-
 .../launch/move_group.launch.py               |   2 +-
 .../launch/moveit_rviz.launch.py              |   4 +-
 .../launch/rsp.launch.py                      |   2 +-
 .../launch/setup_assistant.launch.py          |   7 -
 .../launch/spawn_controllers.launch.py        |   2 +-
 .../launch/static_virtual_joint_tfs.launch.py |   2 +-
 .../launch/ur3e_robotiq.launch.py             | 280 ++++++
 .../launch/ur3e_robotiq_description.launch.py |  40 -
 .../launch/warehouse_db.launch.py             |   7 -
 src/ur_robotiq_moveit_config/package.xml      |  10 +-
 .../{config => rviz}/moveit.rviz              |   0
 .../rviz/view_robot.rviz                      | 285 ++++++
 .../srdf/ur_robotiq_macro.srdf.xacro          |  77 ++
 33 files changed, 901 insertions(+), 1480 deletions(-)
 delete mode 100644 .vscode/c_cpp_properties.json
 delete mode 100644 .vscode/settings.json
 delete mode 100644 src/ur_robotiq_description/urdf/ur_robotiq.urdf
 delete mode 100644 src/ur_robotiq_moveit_config/.setup_assistant
 create mode 100644 src/ur_robotiq_moveit_config/config/controllers.yaml
 delete mode 100644 src/ur_robotiq_moveit_config/config/initial_positions.yaml
 delete mode 100644 src/ur_robotiq_moveit_config/config/moveit_controllers.yaml
 create mode 100644 src/ur_robotiq_moveit_config/config/ompl_planning.yaml
 delete mode 100644 src/ur_robotiq_moveit_config/config/pilz_cartesian_limits.yaml
 delete mode 100644 src/ur_robotiq_moveit_config/config/ros2_controllers.yaml
 delete mode 100644 src/ur_robotiq_moveit_config/config/ur3e_robotiq.ros2_control.xacro
 delete mode 100644 src/ur_robotiq_moveit_config/config/ur3e_robotiq.srdf
 delete mode 100644 src/ur_robotiq_moveit_config/config/ur3e_robotiq.urdf.xacro
 create mode 100644 src/ur_robotiq_moveit_config/config/ur_servo.yaml
 delete mode 100644 src/ur_robotiq_moveit_config/launch/setup_assistant.launch.py
 create mode 100644 src/ur_robotiq_moveit_config/launch/ur3e_robotiq.launch.py
 delete mode 100644 src/ur_robotiq_moveit_config/launch/ur3e_robotiq_description.launch.py
 delete mode 100644 src/ur_robotiq_moveit_config/launch/warehouse_db.launch.py
 rename src/ur_robotiq_moveit_config/{config => rviz}/moveit.rviz (100%)
 create mode 100644 src/ur_robotiq_moveit_config/rviz/view_robot.rviz
 create mode 100644 src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro

diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
deleted file mode 100644
index 13f0a13..0000000
--- a/.vscode/c_cpp_properties.json
+++ /dev/null
@@ -1,66 +0,0 @@
-{
-  "configurations": [
-    {
-      "browse": {
-        "databaseFilename": "${default}",
-        "limitSymbolsToIncludedHeaders": false
-      },
-      "includePath": [
-        "/home/fotis/UR_Robotiq/install/ur_calibration/include/**",
-        "/home/fotis/UR_Robotiq/install/ur_robot_driver/include/**",
-        "/home/fotis/UR_Robotiq/install/ur_controllers/include/**",
-        "/home/fotis/UR_Robotiq/install/ur_dashboard_msgs/include/**",
-        "/home/fotis/UR_Robotiq/install/robotiq_driver/include/**",
-        "/home/fotis/UR_Robotiq/install/serial/include/**",
-        "/home/fotis/UR_Robotiq/install/robotiq_controllers/include/**",
-        "/home/fotis/ws_moveit2/install/pilz_industrial_motion_planner_testutils/include/**",
-        "/home/fotis/ws_moveit2/install/pilz_industrial_motion_planner/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_visual_tools/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_task_constructor_visualization/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_task_constructor_demo/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_task_constructor_core/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_ros_control_interface/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_simple_controller_manager/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_setup_assistant/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_setup_srdf_plugins/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_setup_core_plugins/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_setup_controllers/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_setup_app_plugins/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_setup_framework/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_servo/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_ros_visualization/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_hybrid_planning/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_ros_planning_interface/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_ros_benchmarks/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_ros_warehouse/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_ros_robot_interaction/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_ros_perception/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_ros_move_group/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_planners_ompl/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_ros_planning/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_ros_occupancy_map_monitor/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_kinematics/include/**",
-        "/home/fotis/ws_moveit2/install/chomp_motion_planner/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_core/include/**",
-        "/home/fotis/ws_moveit2/install/srdfdom/include/**",
-        "/home/fotis/ws_moveit2/install/rviz_marker_tools/include/**",
-        "/home/fotis/ws_moveit2/install/rosparam_shortcuts/include/**",
-        "/home/fotis/ws_moveit2/install/moveit_task_constructor_msgs/include/**",
-        "/opt/ros/humble/include/**",
-        "/home/fotis/UR_Robotiq/src/Universal_Robots_ROS2_Driver/ur_calibration/include/**",
-        "/home/fotis/UR_Robotiq/src/Universal_Robots_ROS2_Driver/ur_controllers/include/**",
-        "/home/fotis/UR_Robotiq/src/Universal_Robots_ROS2_Driver/ur_robot_driver/include/**",
-        "/home/fotis/UR_Robotiq/src/ros2_robotiq_gripper/robotiq_controllers/include/**",
-        "/home/fotis/UR_Robotiq/src/ros2_robotiq_gripper/robotiq_driver/include/**",
-        "/home/fotis/UR_Robotiq/src/serial/include/**",
-        "/usr/include/**"
-      ],
-      "name": "ROS",
-      "intelliSenseMode": "gcc-x64",
-      "compilerPath": "/usr/bin/gcc",
-      "cStandard": "gnu11",
-      "cppStandard": "c++14"
-    }
-  ],
-  "version": 4
-}
\ No newline at end of file
diff --git a/.vscode/settings.json b/.vscode/settings.json
deleted file mode 100644
index cea386d..0000000
--- a/.vscode/settings.json
+++ /dev/null
@@ -1,26 +0,0 @@
-{
-    "python.autoComplete.extraPaths": [
-        "/home/fotis/UR_Robotiq/install/ur_robot_driver/local/lib/python3.10/dist-packages",
-        "/home/fotis/UR_Robotiq/install/ur_moveit_config/local/lib/python3.10/dist-packages",
-        "/home/fotis/UR_Robotiq/install/ur_dashboard_msgs/local/lib/python3.10/dist-packages",
-        "/home/fotis/UR_Robotiq/install/my-test-package/lib/python3.10/site-packages",
-        "/home/fotis/ws_moveit2/install/moveit_configs_utils/lib/python3.10/site-packages",
-        "/home/fotis/ws_moveit2/install/srdfdom/local/lib/python3.10/dist-packages",
-        "/home/fotis/ws_moveit2/install/moveit_task_constructor_msgs/local/lib/python3.10/dist-packages",
-        "/home/fotis/ws_moveit2/install/launch_param_builder/lib/python3.10/site-packages",
-        "/opt/ros/humble/lib/python3.10/site-packages",
-        "/opt/ros/humble/local/lib/python3.10/dist-packages"
-    ],
-    "python.analysis.extraPaths": [
-        "/home/fotis/UR_Robotiq/install/ur_robot_driver/local/lib/python3.10/dist-packages",
-        "/home/fotis/UR_Robotiq/install/ur_moveit_config/local/lib/python3.10/dist-packages",
-        "/home/fotis/UR_Robotiq/install/ur_dashboard_msgs/local/lib/python3.10/dist-packages",
-        "/home/fotis/UR_Robotiq/install/my-test-package/lib/python3.10/site-packages",
-        "/home/fotis/ws_moveit2/install/moveit_configs_utils/lib/python3.10/site-packages",
-        "/home/fotis/ws_moveit2/install/srdfdom/local/lib/python3.10/dist-packages",
-        "/home/fotis/ws_moveit2/install/moveit_task_constructor_msgs/local/lib/python3.10/dist-packages",
-        "/home/fotis/ws_moveit2/install/launch_param_builder/lib/python3.10/site-packages",
-        "/opt/ros/humble/lib/python3.10/site-packages",
-        "/opt/ros/humble/local/lib/python3.10/dist-packages"
-    ]
-}
\ No newline at end of file
diff --git a/src/Universal_Robots_ROS2_Driver b/src/Universal_Robots_ROS2_Driver
index 8bc95d7..856a050 160000
--- a/src/Universal_Robots_ROS2_Driver
+++ b/src/Universal_Robots_ROS2_Driver
@@ -1 +1 @@
-Subproject commit 8bc95d773f2476eeb18370a128b9f0c02ee98fd3
+Subproject commit 856a050e2f51bd931226f1e68cf1556ff406cb00
diff --git a/src/ur_robotiq_description/urdf/ur_robotiq.urdf b/src/ur_robotiq_description/urdf/ur_robotiq.urdf
deleted file mode 100644
index 2b63c0c..0000000
--- a/src/ur_robotiq_description/urdf/ur_robotiq.urdf
+++ /dev/null
@@ -1,904 +0,0 @@
-<?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from ur_robotiq.urdf.xacro          | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot name="ur3e_robotiq">
-  <!--
-    Base UR robot series xacro macro.
-
-    NOTE this is NOT a URDF. It cannot directly be loaded by consumers
-    expecting a flattened '.urdf' file. See the top-level '.xacro' for that
-    (but note that .xacro must still be processed by the xacro command).
-
-    This file models the base kinematic chain of a UR robot, which then gets
-    parameterised by various configuration files to convert it into a UR3(e),
-    UR5(e), UR10(e) or UR16e.
-
-    NOTE the default kinematic parameters (i.e., link lengths, frame locations,
-    offsets, etc) do not correspond to any particular robot. They are defaults
-    only. There WILL be non-zero offsets between the Forward Kinematics results
-    in TF (i.e., robot_state_publisher) and the values reported by the Teach
-    Pendant.
-
-    For accurate (and robot-specific) transforms, the 'kinematics_parameters_file'
-    parameter MUST point to a .yaml file containing the appropriate values for
-    the targeted robot.
-
-    If using the UniversalRobots/Universal_Robots_ROS_Driver, follow the steps
-    described in the readme of that repository to extract the kinematic
-    calibration from the controller and generate the required .yaml file.
-
-    Main author of the migration to yaml configs Ludovic Delval.
-
-    Contributors to previous versions (in no particular order)
-
-     - Denis Stogl
-     - Lovro Ivanov
-     - Felix Messmer
-     - Kelsey Hawkins
-     - Wim Meeussen
-     - Shaun Edwards
-     - Nadia Hammoudeh Garcia
-     - Dave Hershberger
-     - G. vd. Hoorn
-     - Philip Long
-     - Dave Coleman
-     - Miguel Prada
-     - Mathias Luedtke
-     - Marcel Schnirring
-     - Felix von Drigalski
-     - Felix Exner
-     - Jimmy Da Silva
-     - Ajit Krisshna N L
-     - Muhammad Asif Rana
-  -->
-  <!--
-    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.
-  -->
-  <link name="world"/>
-  <ros2_control name="ur_arm" type="system">
-    <hardware>
-      <plugin>ur_robot_driver/URPositionHardwareInterface</plugin>
-      <param name="robot_ip">0.0.0.0</param>
-      <param name="script_filename"></param>
-      <param name="output_recipe_filename"></param>
-      <param name="input_recipe_filename"></param>
-      <param name="headless_mode">False</param>
-      <param name="reverse_port">50001</param>
-      <param name="script_sender_port">50002</param>
-      <param name="reverse_ip">0.0.0.0</param>
-      <param name="script_command_port">50004</param>
-      <param name="trajectory_port">50003</param>
-      <param name="tf_prefix"></param>
-      <param name="non_blocking_read">True</param>
-      <param name="servoj_gain">2000</param>
-      <param name="servoj_lookahead_time">0.03</param>
-      <param name="use_tool_communication">False</param>
-      <param name="kinematics/hash">calib_16756443741236045476</param>
-      <param name="tool_voltage">0</param>
-      <param name="tool_parity">0</param>
-      <param name="tool_baud_rate">115200</param>
-      <param name="tool_stop_bits">1</param>
-      <param name="tool_rx_idle_chars">1.5</param>
-      <param name="tool_tx_idle_chars">3.5</param>
-      <param name="tool_device_name">/tmp/ttyUR</param>
-      <param name="tool_tcp_port">54321</param>
-      <param name="keep_alive_count">2</param>
-    </hardware>
-    <joint name="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">0.0</param>
-      </state_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <joint name="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">-1.57</param>
-      </state_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <joint name="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">0.0</param>
-      </state_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <joint name="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">-1.57</param>
-      </state_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <joint name="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">0.0</param>
-      </state_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <joint name="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">0.0</param>
-      </state_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <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="tool_voltage_cmd"/>
-      <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="hand_back_control">
-      <command_interface name="hand_back_control_cmd"/>
-      <command_interface name="hand_back_control_async_success"/>
-    </gpio>
-    <gpio name="zero_ftsensor">
-      <command_interface name="zero_ftsensor_cmd"/>
-      <command_interface name="zero_ftsensor_async_success"/>
-    </gpio>
-    <gpio name="system_interface">
-      <state_interface name="initialized"/>
-    </gpio>
-  </ros2_control>
-  <!-- Add URDF transmission elements (for ros_control) -->
-  <!--<xacro:ur_arm_transmission prefix="${prefix}" hw_interface="${transmission_hw_interface}" />-->
-  <!-- Placeholder for ros2_control transmission which don't yet exist -->
-  <!-- links -  main serial chain -->
-  <link name="base_link"/>
-  <link name="base_link_inertia">
-    <visual>
-      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur3e/visual/base.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur3e/collision/base.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="2.0"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <inertia ixx="0.0030531654454" ixy="0.0" ixz="0.0" iyy="0.0030531654454" iyz="0.0" izz="0.005625"/>
-    </inertial>
-  </link>
-  <link name="shoulder_link">
-    <visual>
-      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur3e/visual/shoulder.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur3e/collision/shoulder.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="2.0"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <inertia ixx="0.008093163429399999" ixy="0.0" ixz="0.0" iyy="0.008093163429399999" iyz="0.0" izz="0.005625"/>
-    </inertial>
-  </link>
-  <link name="upper_arm_link">
-    <visual>
-      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.12"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur3e/visual/upperarm.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.12"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur3e/collision/upperarm.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="3.42"/>
-      <origin rpy="0 1.5707963267948966 0" xyz="-0.121825 0.0 0.12"/>
-      <inertia ixx="0.021728483221103233" ixy="0.0" ixz="0.0" iyy="0.021728483221103233" iyz="0.0" izz="0.00961875"/>
-    </inertial>
-  </link>
-  <link name="forearm_link">
-    <visual>
-      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.027"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur3e/visual/forearm.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.027"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur3e/collision/forearm.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="1.26"/>
-      <origin rpy="0 1.5707963267948966 0" xyz="-0.1066 0.0 0.027"/>
-      <inertia ixx="0.0065445675821719194" ixy="0.0" ixz="0.0" iyy="0.0065445675821719194" iyz="0.0" izz="0.00354375"/>
-    </inertial>
-  </link>
-  <link name="wrist_1_link">
-    <visual>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.104"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur3e/visual/wrist1.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.104"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur3e/collision/wrist1.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="0.8"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <inertia ixx="0.002084999166" ixy="0.0" ixz="0.0" iyy="0.002084999166" iyz="0.0" izz="0.00225"/>
-    </inertial>
-  </link>
-  <link name="wrist_2_link">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 -0.08535"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur3e/visual/wrist2.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 -0.08535"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur3e/collision/wrist2.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="0.8"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <inertia ixx="0.002084999166" ixy="0.0" ixz="0.0" iyy="0.002084999166" iyz="0.0" izz="0.00225"/>
-    </inertial>
-  </link>
-  <link name="wrist_3_link">
-    <visual>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.0921"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur3e/visual/wrist3.dae"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.0921"/>
-      <geometry>
-        <mesh filename="package://ur_description/meshes/ur3e/collision/wrist3.stl"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <mass value="0.35"/>
-      <origin rpy="0 0 0" xyz="0.0 0.0 -0.02"/>
-      <inertia ixx="0.00013626661215999998" ixy="0.0" ixz="0.0" iyy="0.00013626661215999998" iyz="0.0" izz="0.0001792"/>
-    </inertial>
-  </link>
-  <!-- base_joint fixes base_link to the environment -->
-  <joint name="base_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="world"/>
-    <child link="base_link"/>
-  </joint>
-  <!-- joints - main serial chain -->
-  <joint name="base_link-base_link_inertia" type="fixed">
-    <parent link="base_link"/>
-    <child link="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 rpy="0 0 3.141592653589793" xyz="0 0 0"/>
-  </joint>
-  <joint name="shoulder_pan_joint" type="revolute">
-    <parent link="base_link_inertia"/>
-    <child link="shoulder_link"/>
-    <origin rpy="0 0 0" xyz="0 0 0.15185"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
-    <dynamics damping="0" friction="0"/>
-  </joint>
-  <joint name="shoulder_lift_joint" type="revolute">
-    <parent link="shoulder_link"/>
-    <child link="upper_arm_link"/>
-    <origin rpy="1.570796327 0 0" xyz="0 0 0"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
-    <dynamics damping="0" friction="0"/>
-  </joint>
-  <joint name="elbow_joint" type="revolute">
-    <parent link="upper_arm_link"/>
-    <child link="forearm_link"/>
-    <origin rpy="0 0 0" xyz="-0.24355 0 0"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="28.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
-    <dynamics damping="0" friction="0"/>
-  </joint>
-  <joint name="wrist_1_joint" type="revolute">
-    <parent link="forearm_link"/>
-    <child link="wrist_1_link"/>
-    <origin rpy="0 0 0" xyz="-0.2132 0 0.13105"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="12.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="6.283185307179586"/>
-    <dynamics damping="0" friction="0"/>
-  </joint>
-  <joint name="wrist_2_joint" type="revolute">
-    <parent link="wrist_1_link"/>
-    <child link="wrist_2_link"/>
-    <origin rpy="1.570796327 0 0" xyz="0 -0.08535 -1.750557762378351e-11"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="12.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="6.283185307179586"/>
-    <dynamics damping="0" friction="0"/>
-  </joint>
-  <joint name="wrist_3_joint" type="revolute">
-    <parent link="wrist_2_link"/>
-    <child link="wrist_3_link"/>
-    <origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.0921 -1.8890025766262e-11"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="12.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="6.283185307179586"/>
-    <dynamics damping="0" friction="0"/>
-  </joint>
-  <link name="ft_frame"/>
-  <joint name="wrist_3_link-ft_frame" type="fixed">
-    <parent link="wrist_3_link"/>
-    <child link="ft_frame"/>
-    <origin rpy="3.141592653589793 0 0" xyz="0 0 0"/>
-  </joint>
-  <!-- ROS-Industrial 'base' frame - base_link to UR 'Base' Coordinates transform -->
-  <link name="base"/>
-  <joint name="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 rpy="0 0 3.141592653589793" xyz="0 0 0"/>
-    <parent link="base_link"/>
-    <child link="base"/>
-  </joint>
-  <!-- ROS-Industrial 'flange' frame - attachment point for EEF models -->
-  <link name="flange"/>
-  <joint name="wrist_3-flange" type="fixed">
-    <parent link="wrist_3_link"/>
-    <child link="flange"/>
-    <origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
-  </joint>
-  <!-- ROS-Industrial 'tool0' frame - all-zeros tool frame -->
-  <link name="tool0"/>
-  <joint name="flange-tool0" type="fixed">
-    <!-- default toolframe - X+ left, Y+ up, Z+ front -->
-    <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0 0 0"/>
-    <parent link="flange"/>
-    <child link="tool0"/>
-  </joint>
-  <ros2_control name="gripper" type="system">
-    <!-- Plugins -->
-    <hardware>
-      <plugin>robotiq_driver/RobotiqGripperHardwareInterface</plugin>
-      <param name="gripper_closed_position">0.695</param>
-      <param name="COM_port">/dev/ttyUSB0</param>
-      <param name="gripper_speed_multiplier">1.0</param>
-      <param name="gripper_force_multiplier">0.5</param>
-    </hardware>
-    <!-- Joint interfaces -->
-    <!-- With Ignition or Hardware, they handle mimic joints, so we only need this command interface activated -->
-    <joint name="finger_joint">
-      <command_interface name="position"/>
-      <state_interface name="position">
-        <param name="initial_value">0.695</param>
-      </state_interface>
-      <state_interface name="velocity"/>
-    </joint>
-    <gpio name="reactivate_gripper">
-      <command_interface name="reactivate_gripper_cmd"/>
-      <command_interface name="reactivate_gripper_response"/>
-    </gpio>
-  </ros2_control>
-  <!-- this is a temporary link to rotate the 2f-140 gripper to match the 2f-85 -->
-  <link name="robotiq_base_link"/>
-  <joint name="robotiq_base_joint" type="fixed">
-    <parent link="ft_frame"/>
-    <child link="robotiq_base_link"/>
-    <origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/>
-  </joint>
-  <link name="robotiq_140_base_link">
-    <inertial>
-      <origin rpy="0 0 0" xyz="8.625E-08 -4.6583E-06 0.03145"/>
-      <mass value="0.22652"/>
-      <inertia ixx="0.00020005" ixy="-4.2442E-10" ixz="-2.9069E-10" iyy="0.00017832" iyz="-3.4402E-08" izz="0.00013478"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_base_link.stl"/>
-      </geometry>
-      <material name="">
-        <color rgba="0.1 0.1 0.1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_base_link.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="robotiq_140_base_joint" type="fixed">
-    <parent link="robotiq_base_link"/>
-    <child link="robotiq_140_base_link"/>
-    <origin rpy="3.141592653589793 0 0" xyz="0 0 0"/>
-  </joint>
-  <link name="left_outer_knuckle">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331"/>
-      <mass value="0.00853198276973456"/>
-      <inertia ixx="2.89328108496468E-06" ixy="-1.57935047237397E-19" ixz="-1.93980378593255E-19" iyy="1.86719750325683E-06" iyz="-1.21858577871576E-06" izz="1.21905238907251E-06"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
-      </geometry>
-      <material name="">
-        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <link name="left_outer_finger">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385"/>
-      <mass value="0.022614240507152"/>
-      <inertia ixx="1.52518312458174E-05" ixy="9.76583423954399E-10" ixz="-5.43838577022588E-10" iyy="6.17694243867776E-06" iyz="6.78636130740228E-06" izz="1.16494917907219E-05"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl"/>
-      </geometry>
-      <material name="">
-        <color rgba="0.1 0.1 0.1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <link name="left_inner_finger">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257"/>
-      <mass value="0.0104003125914103"/>
-      <inertia ixx="2.71909453810972E-06" ixy="1.35402465472579E-21" ixz="-7.1817349065269E-22" iyy="7.69100314106116E-07" iyz="6.74715432769696E-07" izz="2.30315190420171E-06"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl"/>
-      </geometry>
-      <material name="">
-        <color rgba="0.1 0.1 0.1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <link name="left_inner_finger_pad">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <box size="0.027 0.065 0.0075"/>
-      </geometry>
-      <material name="">
-        <color rgba="0.9 0.9 0.9 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <box size="0.03 0.07 0.0075"/>
-      </geometry>
-      <material name="">
-        <color rgba="0.9 0.0 0.0 1"/>
-      </material>
-    </collision>
-  </link>
-  <link name="left_inner_knuckle">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166"/>
-      <mass value="0.0271177346495152"/>
-      <inertia ixx="2.61910379223783E-05" ixy="-2.43616858946494E-07" ixz="-6.37789906117123E-09" iyy="2.8270243746167E-06" iyz="-5.37200748039765E-07" izz="2.83695868220296E-05"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
-      </geometry>
-      <material name="">
-        <color rgba="0.1 0.1 0.1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <link name="right_outer_knuckle">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331"/>
-      <mass value="0.00853198276973456"/>
-      <inertia ixx="2.89328108496468E-06" ixy="-1.57935047237397E-19" ixz="-1.93980378593255E-19" iyy="1.86719750325683E-06" iyz="-1.21858577871576E-06" izz="1.21905238907251E-06"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
-      </geometry>
-      <material name="">
-        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <link name="right_outer_finger">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385"/>
-      <mass value="0.022614240507152"/>
-      <inertia ixx="1.52518312458174E-05" ixy="9.76583423954399E-10" ixz="-5.43838577022588E-10" iyy="6.17694243867776E-06" iyz="6.78636130740228E-06" izz="1.16494917907219E-05"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl"/>
-      </geometry>
-      <material name="">
-        <color rgba="0.1 0.1 0.1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <link name="right_inner_finger">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257"/>
-      <mass value="0.0104003125914103"/>
-      <inertia ixx="2.71909453810972E-06" ixy="1.35402465472579E-21" ixz="-7.1817349065269E-22" iyy="7.69100314106116E-07" iyz="6.74715432769696E-07" izz="2.30315190420171E-06"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl"/>
-      </geometry>
-      <material name="">
-        <color rgba="0.1 0.1 0.1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <link name="right_inner_finger_pad">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <box size="0.027 0.065 0.0075"/>
-      </geometry>
-      <material name="">
-        <color rgba="0.9 0.9 0.9 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <box size="0.03 0.07 0.0075"/>
-      </geometry>
-      <material name="">
-        <color rgba="0.9 0.0 0.0 1"/>
-      </material>
-    </collision>
-  </link>
-  <link name="right_inner_knuckle">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166"/>
-      <mass value="0.0271177346495152"/>
-      <inertia ixx="2.61910379223783E-05" ixy="-2.43616858946494E-07" ixz="-6.37789906117123E-09" iyy="2.8270243746167E-06" iyz="-5.37200748039765E-07" izz="2.83695868220296E-05"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
-      </geometry>
-      <material name="">
-        <color rgba="0.1 0.1 0.1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="finger_joint" type="revolute">
-    <origin rpy="2.2957963267948966 0 0" xyz="0 -0.030601 0.054905"/>
-    <parent link="robotiq_140_base_link"/>
-    <child link="left_outer_knuckle"/>
-    <axis xyz="-1 0 0"/>
-    <limit effort="1000" lower="0" upper="0.7" velocity="2.0"/>
-  </joint>
-  <joint name="left_outer_finger_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0.01821998610742 0.0260018192872234"/>
-    <parent link="left_outer_knuckle"/>
-    <child link="left_outer_finger"/>
-    <axis xyz="1 0 0"/>
-  </joint>
-  <joint name="left_inner_knuckle_joint" type="revolute">
-    <origin rpy="2.2957963267948966 0 0.0" xyz="0 -0.0127 0.06142"/>
-    <parent link="robotiq_140_base_link"/>
-    <child link="left_inner_knuckle"/>
-    <axis xyz="1 0 0"/>
-    <limit effort="1000" lower="-0.8757" upper="0.8757" velocity="2.0"/>
-    <mimic joint="finger_joint" multiplier="-1" offset="0"/>
-  </joint>
-  <joint name="left_inner_finger_joint" type="revolute">
-    <origin rpy="-0.725 0 0" xyz="0 0.0817554015893473 -0.0282203446692936"/>
-    <parent link="left_outer_finger"/>
-    <child link="left_inner_finger"/>
-    <axis xyz="1 0 0"/>
-    <limit effort="1000" lower="-0.8757" upper="0.8757" velocity="2.0"/>
-    <mimic joint="finger_joint" multiplier="1" offset="0"/>
-  </joint>
-  <joint name="left_inner_finger_pad_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0.0457554015893473 -0.0272203446692936"/>
-    <parent link="left_inner_finger"/>
-    <child link="left_inner_finger_pad"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <joint name="right_outer_knuckle_joint" type="revolute">
-    <origin rpy="2.2957963267948966 0 3.141592653589793" xyz="0 0.030601 0.054905"/>
-    <parent link="robotiq_140_base_link"/>
-    <child link="right_outer_knuckle"/>
-    <axis xyz="1 0 0"/>
-    <limit effort="1000" lower="-0.725" upper="0.725" velocity="2.0"/>
-    <mimic joint="finger_joint" multiplier="-1" offset="0"/>
-  </joint>
-  <joint name="right_outer_finger_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0.01821998610742 0.0260018192872234"/>
-    <parent link="right_outer_knuckle"/>
-    <child link="right_outer_finger"/>
-    <axis xyz="1 0 0"/>
-  </joint>
-  <joint name="right_inner_knuckle_joint" type="revolute">
-    <origin rpy="2.2957963267948966 0 -3.141592653589793" xyz="0 0.0127 0.06142"/>
-    <parent link="robotiq_140_base_link"/>
-    <child link="right_inner_knuckle"/>
-    <axis xyz="1 0 0"/>
-    <limit effort="1000" lower="-0.8757" upper="0.8757" velocity="2.0"/>
-    <mimic joint="finger_joint" multiplier="-1" offset="0"/>
-  </joint>
-  <joint name="right_inner_finger_joint" type="revolute">
-    <origin rpy="-0.725 0 0" xyz="0 0.0817554015893473 -0.0282203446692936"/>
-    <parent link="right_outer_finger"/>
-    <child link="right_inner_finger"/>
-    <axis xyz="1 0 0"/>
-    <limit effort="1000" lower="-0.8757" upper="0.8757" velocity="2.0"/>
-    <mimic joint="finger_joint" multiplier="1" offset="0"/>
-  </joint>
-  <joint name="right_inner_finger_pad_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0.0457554015893473 -0.0272203446692936"/>
-    <parent link="right_inner_finger"/>
-    <child link="right_inner_finger_pad"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <joint name="ft_frame-gripper" type="fixed">
-    <parent link="ft_frame"/>
-    <child link="robotiq_base_link"/>
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-  </joint>
-</robot>
diff --git a/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro b/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro
index d2d05e0..0f19577 100644
--- a/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro
+++ b/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro
@@ -1,10 +1,11 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://wiki.ros.org/xacro" name="ur3e_robotiq">
+<robot xmlns:xacro="http://wiki.ros.org/xacro" name="ur_robotiq">
 
 	<!-- parameters -->    
-	<xacro:arg name="name" default="ur_arm"/>
+	<xacro:arg name="name" default="ur"/>
 	<!-- import main macro -->
 	<xacro:include filename="$(find ur_description)/urdf/ur_macro.xacro"/>
+	<xacro:include filename="$(find robotiq_description)/urdf/ur_to_robotiq_adapter.urdf.xacro" />
 	<xacro:include filename="$(find robotiq_description)/urdf/robotiq_2f_140_macro.urdf.xacro" />
 
 	<!-- possible 'ur_type' values: ur3, ur3e, ur5, ur5e, ur10, ur10e, ur16e, ur20 -->
@@ -57,54 +58,59 @@
 	<xacro:property name="initial_positions_file" default="$(arg initial_positions_file)"/>	
 
  	<link name="world"/>
+
 	<xacro:ur_robot
-	     name="$(arg name)"
-	     tf_prefix="$(arg tf_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_ignition="$(arg sim_ignition)"
-	     headless_mode="$(arg headless_mode)"
-	     initial_positions="${xacro.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)"
-	     reverse_ip="$(arg reverse_ip)"
-	     script_command_port="$(arg script_command_port)"
-	     reverse_port="$(arg reverse_port)"
-	     script_sender_port="$(arg script_sender_port)"
-	     trajectory_port="$(arg trajectory_port)"
-	     >
-	     <origin xyz="0 0 0" rpy="0 0 0" />          <!-- position robot in the world -->
-	   </xacro:ur_robot>
+     name="$(arg name)"
+     tf_prefix="$(arg tf_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_ignition="$(arg sim_ignition)"
+     headless_mode="$(arg headless_mode)"
+     initial_positions="${xacro.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)"
+     reverse_ip="$(arg reverse_ip)"
+     script_command_port="$(arg script_command_port)"
+     reverse_port="$(arg reverse_port)"
+     script_sender_port="$(arg script_sender_port)"
+     trajectory_port="$(arg trajectory_port)"
+     >
+    <origin xyz="0 0 0" rpy="0 0 0" />          <!-- position robot in the world -->
+    </xacro:ur_robot>
 
-	<xacro:robotiq_gripper name="gripper" prefix="" parent="ft_frame" use_fake_hardware="$(arg use_fake_hardware)">
-	<origin xyz="0 0 0" rpy="3.141592653589793 0 0" />
-	    </xacro:robotiq_gripper>
 
-	<joint name="ft_frame-gripper" type="fixed">
-		<parent link="ft_frame"/> 
-		<child link="robotiq_base_link"/>
-		<origin xyz="0 0 0" rpy="0 0 0"/> 
-	</joint>
+   <xacro:robotiq_gripper name="$(arg tf_prefix)gripper"
+    prefix="$(arg tf_prefix)"
+    parent="$(arg tf_prefix)ur_to_robotiq_link"
+    use_fake_hardware="$(arg use_fake_hardware)"
+    >
+    <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:robotiq_gripper>
 
+   <xacro:ur_to_robotiq prefix="$(arg tf_prefix)"
+     parent="$(arg tf_prefix)tool0"
+     child="$(arg tf_prefix)robotiq_base_link"
+     >
+   </xacro:ur_to_robotiq>
 </robot>
diff --git a/src/ur_robotiq_moveit_config/.setup_assistant b/src/ur_robotiq_moveit_config/.setup_assistant
deleted file mode 100644
index 411385c..0000000
--- a/src/ur_robotiq_moveit_config/.setup_assistant
+++ /dev/null
@@ -1,27 +0,0 @@
-moveit_setup_assistant_config:
-  urdf:
-    package: ur_robotiq_description
-    relative_path: urdf/ur_robotiq.urdf
-  srdf:
-    relative_path: config/ur3e_robotiq.srdf
-  package_settings:
-    author_name: Fotios Lygerakis
-    author_email: fotios.lygerakis@unileoben.ac.at
-    generated_timestamp: 1709822028
-  control_xacro:
-    command:
-      - position
-      - velocity
-    state:
-      - position
-      - velocity
-  modified_urdf:
-    xacros:
-      - control_xacro
-  control_xacro:
-    command:
-      - position
-      - velocity
-    state:
-      - position
-      - velocity
\ No newline at end of file
diff --git a/src/ur_robotiq_moveit_config/CMakeLists.txt b/src/ur_robotiq_moveit_config/CMakeLists.txt
index 2c40b44..d156ca7 100644
--- a/src/ur_robotiq_moveit_config/CMakeLists.txt
+++ b/src/ur_robotiq_moveit_config/CMakeLists.txt
@@ -8,4 +8,5 @@ ament_package()
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}
   PATTERN "setup_assistant.launch" EXCLUDE)
 install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
-install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY srdf DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME})
diff --git a/src/ur_robotiq_moveit_config/config/controllers.yaml b/src/ur_robotiq_moveit_config/config/controllers.yaml
new file mode 100644
index 0000000..784ebd3
--- /dev/null
+++ b/src/ur_robotiq_moveit_config/config/controllers.yaml
@@ -0,0 +1,29 @@
+controller_names:
+  - scaled_joint_trajectory_controller
+  - joint_trajectory_controller
+
+
+scaled_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
+
+
+joint_trajectory_controller:
+  action_ns: follow_joint_trajectory
+  type: FollowJointTrajectory
+  default: false
+  joints:
+    - shoulder_pan_joint
+    - shoulder_lift_joint
+    - elbow_joint
+    - wrist_1_joint
+    - wrist_2_joint
+    - wrist_3_joint
diff --git a/src/ur_robotiq_moveit_config/config/initial_positions.yaml b/src/ur_robotiq_moveit_config/config/initial_positions.yaml
deleted file mode 100644
index 2e29228..0000000
--- a/src/ur_robotiq_moveit_config/config/initial_positions.yaml
+++ /dev/null
@@ -1,10 +0,0 @@
-# Default initial positions for ur3e_robotiq's ros2_control fake system
-
-initial_positions:
-  elbow_joint: 0
-  finger_joint: 0
-  shoulder_lift_joint: 0
-  shoulder_pan_joint: 0
-  wrist_1_joint: 0
-  wrist_2_joint: 0
-  wrist_3_joint: 0
\ No newline at end of file
diff --git a/src/ur_robotiq_moveit_config/config/joint_limits.yaml b/src/ur_robotiq_moveit_config/config/joint_limits.yaml
index bc7e250..9021a01 100644
--- a/src/ur_robotiq_moveit_config/config/joint_limits.yaml
+++ b/src/ur_robotiq_moveit_config/config/joint_limits.yaml
@@ -13,7 +13,12 @@ joint_limits:
     max_velocity: 3.1415926535897931
     has_acceleration_limits: false
     max_acceleration: 0
-  finger_joint:
+  left_inner_knuckle_joint:
+    has_velocity_limits: true
+    max_velocity: 2
+    has_acceleration_limits: false
+    max_acceleration: 0
+  right_inner_knuckle_joint:
     has_velocity_limits: true
     max_velocity: 2
     has_acceleration_limits: false
diff --git a/src/ur_robotiq_moveit_config/config/kinematics.yaml b/src/ur_robotiq_moveit_config/config/kinematics.yaml
index 2f6bb96..654fb9d 100644
--- a/src/ur_robotiq_moveit_config/config/kinematics.yaml
+++ b/src/ur_robotiq_moveit_config/config/kinematics.yaml
@@ -1,4 +1,12 @@
-arm:
-  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
-  kinematics_solver_search_resolution: 0.0050000000000000001
-  kinematics_solver_timeout: 0.0050000000000000001
\ No newline at end of file
+ur_manipulator:
+  ros__parameters:
+    kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+    kinematics_solver_search_resolution: 0.005
+    kinematics_solver_timeout: 0.005
+    kinematics_solver_attempts: 3
+robotiq_gripper:
+  ros__parameters:
+    kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+    kinematics_solver_search_resolution: 0.005
+    kinematics_solver_timeout: 0.005
+    kinematics_solver_attempts: 3
\ No newline at end of file
diff --git a/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml b/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml
deleted file mode 100644
index 5b39bf6..0000000
--- a/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-# MoveIt uses this configuration for controller management
-
-moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
-
-moveit_simple_controller_manager:
-  controller_names:
-    - arm_controller
-    - gripper_controller
-
-  arm_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:
-      - finger_joint
\ No newline at end of file
diff --git a/src/ur_robotiq_moveit_config/config/ompl_planning.yaml b/src/ur_robotiq_moveit_config/config/ompl_planning.yaml
new file mode 100644
index 0000000..5fbbcf5
--- /dev/null
+++ b/src/ur_robotiq_moveit_config/config/ompl_planning.yaml
@@ -0,0 +1,70 @@
+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
+ur_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/src/ur_robotiq_moveit_config/config/pilz_cartesian_limits.yaml b/src/ur_robotiq_moveit_config/config/pilz_cartesian_limits.yaml
deleted file mode 100644
index b2997ca..0000000
--- a/src/ur_robotiq_moveit_config/config/pilz_cartesian_limits.yaml
+++ /dev/null
@@ -1,6 +0,0 @@
-# Limits for the Pilz planner
-cartesian_limits:
-  max_trans_vel: 1.0
-  max_trans_acc: 2.25
-  max_trans_dec: -5.0
-  max_rot_vel: 1.57
diff --git a/src/ur_robotiq_moveit_config/config/ros2_controllers.yaml b/src/ur_robotiq_moveit_config/config/ros2_controllers.yaml
deleted file mode 100644
index 20bb825..0000000
--- a/src/ur_robotiq_moveit_config/config/ros2_controllers.yaml
+++ /dev/null
@@ -1,41 +0,0 @@
-# This config file is used by ros2_control
-controller_manager:
-  ros__parameters:
-    update_rate: 100  # Hz
-
-    arm_controller:
-      type: joint_trajectory_controller/JointTrajectoryController
-
-
-    gripper_controller:
-      type: joint_trajectory_controller/JointTrajectoryController
-
-
-    joint_state_broadcaster:
-      type: joint_state_broadcaster/JointStateBroadcaster
-
-arm_controller:
-  ros__parameters:
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-    command_interfaces:
-      - position
-      - velocity
-    state_interfaces:
-      - position
-      - velocity
-gripper_controller:
-  ros__parameters:
-    joints:
-      - finger_joint
-    command_interfaces:
-      - position
-      - velocity
-    state_interfaces:
-      - position
-      - velocity
\ No newline at end of file
diff --git a/src/ur_robotiq_moveit_config/config/ur3e_robotiq.ros2_control.xacro b/src/ur_robotiq_moveit_config/config/ur3e_robotiq.ros2_control.xacro
deleted file mode 100644
index 4c73aec..0000000
--- a/src/ur_robotiq_moveit_config/config/ur3e_robotiq.ros2_control.xacro
+++ /dev/null
@@ -1,70 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-    <xacro:macro name="ur3e_robotiq_ros2_control" params="name initial_positions_file">
-        <xacro:property name="initial_positions" value="${load_yaml(initial_positions_file)['initial_positions']}"/>
-
-        <ros2_control name="${name}" type="system">
-            <hardware>
-                <!-- By default, set up controllers for simulation. This won't work on real hardware -->
-                <plugin>mock_components/GenericSystem</plugin>
-            </hardware>
-            <joint name="shoulder_pan_joint">
-                <command_interface name="position"/>
-                <command_interface name="velocity"/>
-                <state_interface name="position">
-                  <param name="initial_value">${initial_positions['shoulder_pan_joint']}</param>
-                </state_interface>
-                <state_interface name="velocity"/>
-            </joint>
-            <joint name="shoulder_lift_joint">
-                <command_interface name="position"/>
-                <command_interface name="velocity"/>
-                <state_interface name="position">
-                  <param name="initial_value">${initial_positions['shoulder_lift_joint']}</param>
-                </state_interface>
-                <state_interface name="velocity"/>
-            </joint>
-            <joint name="elbow_joint">
-                <command_interface name="position"/>
-                <command_interface name="velocity"/>
-                <state_interface name="position">
-                  <param name="initial_value">${initial_positions['elbow_joint']}</param>
-                </state_interface>
-                <state_interface name="velocity"/>
-            </joint>
-            <joint name="wrist_1_joint">
-                <command_interface name="position"/>
-                <command_interface name="velocity"/>
-                <state_interface name="position">
-                  <param name="initial_value">${initial_positions['wrist_1_joint']}</param>
-                </state_interface>
-                <state_interface name="velocity"/>
-            </joint>
-            <joint name="wrist_2_joint">
-                <command_interface name="position"/>
-                <command_interface name="velocity"/>
-                <state_interface name="position">
-                  <param name="initial_value">${initial_positions['wrist_2_joint']}</param>
-                </state_interface>
-                <state_interface name="velocity"/>
-            </joint>
-            <joint name="wrist_3_joint">
-                <command_interface name="position"/>
-                <command_interface name="velocity"/>
-                <state_interface name="position">
-                  <param name="initial_value">${initial_positions['wrist_3_joint']}</param>
-                </state_interface>
-                <state_interface name="velocity"/>
-            </joint>
-            <joint name="finger_joint">
-                <command_interface name="position"/>
-                <command_interface name="velocity"/>
-                <state_interface name="position">
-                  <param name="initial_value">${initial_positions['finger_joint']}</param>
-                </state_interface>
-                <state_interface name="velocity"/>
-            </joint>
-
-        </ros2_control>
-    </xacro:macro>
-</robot>
diff --git a/src/ur_robotiq_moveit_config/config/ur3e_robotiq.srdf b/src/ur_robotiq_moveit_config/config/ur3e_robotiq.srdf
deleted file mode 100644
index 537e9b8..0000000
--- a/src/ur_robotiq_moveit_config/config/ur3e_robotiq.srdf
+++ /dev/null
@@ -1,164 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<!--This does not replace URDF, and is not an extension of URDF.
-    This is a format for representing semantic information about the robot structure.
-    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
--->
-<robot name="ur3e_robotiq">
-    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
-    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
-    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
-    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
-    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
-    <group name="arm">
-        <joint name="shoulder_pan_joint"/>
-        <joint name="shoulder_lift_joint"/>
-        <joint name="elbow_joint"/>
-        <joint name="wrist_1_joint"/>
-        <joint name="wrist_2_joint"/>
-        <joint name="wrist_3_joint"/>
-    </group>
-    <group name="gripper">
-        <joint name="finger_joint"/>
-    </group>
-    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
-    <group_state name="arm_default" group="arm">
-        <joint name="elbow_joint" value="0"/>
-        <joint name="shoulder_lift_joint" value="0"/>
-        <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 name="arm_up" group="arm">
-        <joint name="elbow_joint" value="0"/>
-        <joint name="shoulder_lift_joint" value="-1.57"/>
-        <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 name="gripper_open" group="gripper">
-        <joint name="finger_joint" value="0"/>
-    </group_state>
-    <group_state name="gripper_closed" group="gripper">
-        <joint name="finger_joint" value="0.7"/>
-    </group_state>
-    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
-    <end_effector name="ee" parent_link="wrist_3_link" group="gripper" parent_group="arm"/>
-    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
-    <virtual_joint name="virtual_base" type="fixed" parent_frame="world" child_link="base_link"/>
-    <!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
-    <passive_joint name="left_inner_finger_joint"/>
-    <passive_joint name="left_inner_knuckle_joint"/>
-    <passive_joint name="right_inner_knuckle_joint"/>
-    <passive_joint name="right_outer_knuckle_joint"/>
-    <passive_joint name="right_inner_finger_joint"/>
-    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
-    <disable_collisions link1="base_link_inertia" link2="shoulder_link" reason="Adjacent"/>
-    <disable_collisions link1="base_link_inertia" link2="upper_arm_link" reason="Never"/>
-    <disable_collisions link1="base_link_inertia" link2="wrist_1_link" reason="Never"/>
-    <disable_collisions link1="base_link_inertia" link2="wrist_2_link" reason="Never"/>
-    <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="left_inner_finger" link2="left_inner_finger" reason="Default"/>
-    <disable_collisions link1="left_inner_finger" link2="left_inner_finger_pad" reason="Adjacent"/>
-    <disable_collisions link1="left_inner_finger" link2="left_inner_knuckle" reason="Default"/>
-    <disable_collisions link1="left_inner_finger" link2="left_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="left_inner_finger" link2="left_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="left_inner_finger" link2="right_inner_finger" reason="Never"/>
-    <disable_collisions link1="left_inner_finger" link2="right_inner_finger_pad" reason="Never"/>
-    <disable_collisions link1="left_inner_finger" link2="right_inner_knuckle" reason="Never"/>
-    <disable_collisions link1="left_inner_finger" link2="right_outer_finger" reason="Never"/>
-    <disable_collisions link1="left_inner_finger" link2="right_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="left_inner_finger" link2="robotiq_140_base_link" reason="Never"/>
-    <disable_collisions link1="left_inner_finger" link2="wrist_1_link" reason="Never"/>
-    <disable_collisions link1="left_inner_finger" link2="wrist_2_link" reason="Never"/>
-    <disable_collisions link1="left_inner_finger" link2="wrist_3_link" reason="Never"/>
-    <disable_collisions link1="left_inner_finger_pad" link2="left_inner_finger_pad" reason="Default"/>
-    <disable_collisions link1="left_inner_finger_pad" link2="left_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="left_inner_finger_pad" link2="right_inner_finger" reason="Never"/>
-    <disable_collisions link1="left_inner_finger_pad" link2="right_inner_knuckle" reason="Never"/>
-    <disable_collisions link1="left_inner_finger_pad" link2="right_outer_finger" reason="Never"/>
-    <disable_collisions link1="left_inner_finger_pad" link2="right_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="left_inner_finger_pad" link2="robotiq_140_base_link" reason="Never"/>
-    <disable_collisions link1="left_inner_finger_pad" link2="wrist_1_link" reason="Never"/>
-    <disable_collisions link1="left_inner_finger_pad" link2="wrist_2_link" reason="Never"/>
-    <disable_collisions link1="left_inner_finger_pad" link2="wrist_3_link" reason="Never"/>
-    <disable_collisions link1="left_inner_knuckle" link2="left_inner_knuckle" reason="Default"/>
-    <disable_collisions link1="left_inner_knuckle" link2="left_outer_knuckle" reason="Default"/>
-    <disable_collisions link1="left_inner_knuckle" link2="right_inner_finger" reason="Never"/>
-    <disable_collisions link1="left_inner_knuckle" link2="right_inner_finger_pad" reason="Never"/>
-    <disable_collisions link1="left_inner_knuckle" link2="right_inner_knuckle" reason="Never"/>
-    <disable_collisions link1="left_inner_knuckle" link2="right_outer_finger" reason="Never"/>
-    <disable_collisions link1="left_inner_knuckle" link2="right_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="left_inner_knuckle" link2="robotiq_140_base_link" reason="Adjacent"/>
-    <disable_collisions link1="left_inner_knuckle" link2="wrist_1_link" reason="Never"/>
-    <disable_collisions link1="left_inner_knuckle" link2="wrist_2_link" reason="Never"/>
-    <disable_collisions link1="left_inner_knuckle" link2="wrist_3_link" reason="Never"/>
-    <disable_collisions link1="left_outer_finger" link2="left_outer_finger" reason="Default"/>
-    <disable_collisions link1="left_outer_finger" link2="left_outer_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="left_outer_finger" link2="right_inner_finger" reason="Never"/>
-    <disable_collisions link1="left_outer_finger" link2="right_inner_finger_pad" reason="Never"/>
-    <disable_collisions link1="left_outer_finger" link2="right_inner_knuckle" reason="Never"/>
-    <disable_collisions link1="left_outer_finger" link2="right_outer_finger" reason="Never"/>
-    <disable_collisions link1="left_outer_finger" link2="right_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="left_outer_finger" link2="robotiq_140_base_link" reason="Never"/>
-    <disable_collisions link1="left_outer_finger" link2="wrist_1_link" reason="Never"/>
-    <disable_collisions link1="left_outer_finger" link2="wrist_2_link" reason="Never"/>
-    <disable_collisions link1="left_outer_finger" link2="wrist_3_link" reason="Never"/>
-    <disable_collisions link1="left_outer_knuckle" link2="left_outer_knuckle" reason="Default"/>
-    <disable_collisions link1="left_outer_knuckle" link2="right_inner_finger" reason="Never"/>
-    <disable_collisions link1="left_outer_knuckle" link2="right_inner_finger_pad" reason="Never"/>
-    <disable_collisions link1="left_outer_knuckle" link2="right_inner_knuckle" reason="Never"/>
-    <disable_collisions link1="left_outer_knuckle" link2="right_outer_finger" reason="Never"/>
-    <disable_collisions link1="left_outer_knuckle" link2="right_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="left_outer_knuckle" link2="robotiq_140_base_link" reason="Adjacent"/>
-    <disable_collisions link1="left_outer_knuckle" link2="wrist_1_link" reason="Never"/>
-    <disable_collisions link1="left_outer_knuckle" link2="wrist_2_link" reason="Never"/>
-    <disable_collisions link1="left_outer_knuckle" link2="wrist_3_link" reason="Never"/>
-    <disable_collisions link1="right_inner_finger" link2="right_inner_finger" reason="Default"/>
-    <disable_collisions link1="right_inner_finger" link2="right_inner_finger_pad" reason="Adjacent"/>
-    <disable_collisions link1="right_inner_finger" link2="right_inner_knuckle" reason="Default"/>
-    <disable_collisions link1="right_inner_finger" link2="right_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="right_inner_finger" link2="right_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="right_inner_finger" link2="robotiq_140_base_link" reason="Never"/>
-    <disable_collisions link1="right_inner_finger" link2="wrist_1_link" reason="Never"/>
-    <disable_collisions link1="right_inner_finger" link2="wrist_2_link" reason="Never"/>
-    <disable_collisions link1="right_inner_finger" link2="wrist_3_link" reason="Never"/>
-    <disable_collisions link1="right_inner_finger_pad" link2="right_inner_finger_pad" reason="Default"/>
-    <disable_collisions link1="right_inner_finger_pad" link2="right_inner_knuckle" reason="Never"/>
-    <disable_collisions link1="right_inner_finger_pad" link2="right_outer_finger" reason="Never"/>
-    <disable_collisions link1="right_inner_finger_pad" link2="right_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="right_inner_finger_pad" link2="robotiq_140_base_link" reason="Never"/>
-    <disable_collisions link1="right_inner_finger_pad" link2="wrist_1_link" reason="Never"/>
-    <disable_collisions link1="right_inner_finger_pad" link2="wrist_2_link" reason="Never"/>
-    <disable_collisions link1="right_inner_finger_pad" link2="wrist_3_link" reason="Never"/>
-    <disable_collisions link1="right_inner_knuckle" link2="right_inner_knuckle" reason="Default"/>
-    <disable_collisions link1="right_inner_knuckle" link2="right_outer_finger" reason="Never"/>
-    <disable_collisions link1="right_inner_knuckle" link2="right_outer_knuckle" reason="Default"/>
-    <disable_collisions link1="right_inner_knuckle" link2="robotiq_140_base_link" reason="Adjacent"/>
-    <disable_collisions link1="right_inner_knuckle" link2="wrist_1_link" reason="Never"/>
-    <disable_collisions link1="right_inner_knuckle" link2="wrist_2_link" reason="Never"/>
-    <disable_collisions link1="right_inner_knuckle" link2="wrist_3_link" reason="Never"/>
-    <disable_collisions link1="right_outer_finger" link2="right_outer_finger" reason="Default"/>
-    <disable_collisions link1="right_outer_finger" link2="right_outer_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="right_outer_finger" link2="robotiq_140_base_link" reason="Never"/>
-    <disable_collisions link1="right_outer_finger" link2="wrist_1_link" reason="Never"/>
-    <disable_collisions link1="right_outer_finger" link2="wrist_2_link" reason="Never"/>
-    <disable_collisions link1="right_outer_finger" link2="wrist_3_link" reason="Never"/>
-    <disable_collisions link1="right_outer_knuckle" link2="right_outer_knuckle" reason="Default"/>
-    <disable_collisions link1="right_outer_knuckle" link2="robotiq_140_base_link" reason="Adjacent"/>
-    <disable_collisions link1="right_outer_knuckle" link2="wrist_1_link" reason="Never"/>
-    <disable_collisions link1="right_outer_knuckle" link2="wrist_2_link" reason="Never"/>
-    <disable_collisions link1="right_outer_knuckle" link2="wrist_3_link" reason="Never"/>
-    <disable_collisions link1="robotiq_140_base_link" link2="robotiq_140_base_link" reason="Adjacent"/>
-    <disable_collisions link1="robotiq_140_base_link" link2="wrist_1_link" reason="Never"/>
-    <disable_collisions link1="robotiq_140_base_link" link2="wrist_2_link" reason="Never"/>
-    <disable_collisions link1="robotiq_140_base_link" link2="wrist_3_link" reason="Adjacent"/>
-    <disable_collisions link1="shoulder_link" link2="upper_arm_link" reason="Adjacent"/>
-    <disable_collisions link1="shoulder_link" link2="wrist_1_link" reason="Never"/>
-    <disable_collisions link1="shoulder_link" link2="wrist_2_link" reason="Never"/>
-    <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>
diff --git a/src/ur_robotiq_moveit_config/config/ur3e_robotiq.urdf.xacro b/src/ur_robotiq_moveit_config/config/ur3e_robotiq.urdf.xacro
deleted file mode 100644
index f7bf8ed..0000000
--- a/src/ur_robotiq_moveit_config/config/ur3e_robotiq.urdf.xacro
+++ /dev/null
@@ -1,14 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="ur3e_robotiq">
-    <xacro:arg name="initial_positions_file" default="initial_positions.yaml" />
-
-    <!-- Import ur3e_robotiq urdf file -->
-    <xacro:include filename="$(find ur_robotiq_description)/urdf/ur_robotiq.urdf" />
-
-    <!-- Import control_xacro -->
-    <xacro:include filename="ur3e_robotiq.ros2_control.xacro" />
-
-
-    <xacro:ur3e_robotiq_ros2_control name="FakeSystem" initial_positions_file="$(arg initial_positions_file)"/>
-
-</robot>
diff --git a/src/ur_robotiq_moveit_config/config/ur_servo.yaml b/src/ur_robotiq_moveit_config/config/ur_servo.yaml
new file mode 100644
index 0000000..9802702
--- /dev/null
+++ b/src/ur_robotiq_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:  ur_manipulator  # 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/src/ur_robotiq_moveit_config/launch/demo.launch.py b/src/ur_robotiq_moveit_config/launch/demo.launch.py
index b714053..d848b95 100644
--- a/src/ur_robotiq_moveit_config/launch/demo.launch.py
+++ b/src/ur_robotiq_moveit_config/launch/demo.launch.py
@@ -3,7 +3,5 @@ from moveit_configs_utils.launches import generate_demo_launch
 
 
 def generate_launch_description():
-    builder = MoveItConfigsBuilder("ur3_robotiq_2f_140.urdf", robot_description= "urdf/ur_robotiq.urdf", package_name="ur_robotiq_moveit_config")
-    print(builder)
-    moveit_config = builder.to_moveit_configs()
+    moveit_config = MoveItConfigsBuilder("ur_robotiq", package_name="ur_robotiq_moveit_config").to_moveit_configs()
     return generate_demo_launch(moveit_config)
diff --git a/src/ur_robotiq_moveit_config/launch/move_group.launch.py b/src/ur_robotiq_moveit_config/launch/move_group.launch.py
index 38076e8..a996b59 100644
--- a/src/ur_robotiq_moveit_config/launch/move_group.launch.py
+++ b/src/ur_robotiq_moveit_config/launch/move_group.launch.py
@@ -3,5 +3,5 @@ from moveit_configs_utils.launches import generate_move_group_launch
 
 
 def generate_launch_description():
-    moveit_config = MoveItConfigsBuilder("ur3e_robotiq", package_name="ur_robotiq_moveit_config").to_moveit_configs()
+    moveit_config = MoveItConfigsBuilder("ur_robotiq", package_name="ur_robotiq_moveit_config").to_moveit_configs()
     return generate_move_group_launch(moveit_config)
diff --git a/src/ur_robotiq_moveit_config/launch/moveit_rviz.launch.py b/src/ur_robotiq_moveit_config/launch/moveit_rviz.launch.py
index 72a9368..809eaf2 100644
--- a/src/ur_robotiq_moveit_config/launch/moveit_rviz.launch.py
+++ b/src/ur_robotiq_moveit_config/launch/moveit_rviz.launch.py
@@ -3,7 +3,5 @@ from moveit_configs_utils.launches import generate_moveit_rviz_launch
 
 
 def generate_launch_description():
-    moveit_config = MoveItConfigsBuilder("ur3e_robotiq", package_name="ur_robotiq_moveit_config").to_moveit_configs()
-    print(moveit_config.robot_description_kinematics)
+    moveit_config = MoveItConfigsBuilder("ur_robotiq", package_name="ur_robotiq_moveit_config").to_moveit_configs()
     return generate_moveit_rviz_launch(moveit_config)
-
diff --git a/src/ur_robotiq_moveit_config/launch/rsp.launch.py b/src/ur_robotiq_moveit_config/launch/rsp.launch.py
index b2dfc9c..adbac45 100644
--- a/src/ur_robotiq_moveit_config/launch/rsp.launch.py
+++ b/src/ur_robotiq_moveit_config/launch/rsp.launch.py
@@ -3,5 +3,5 @@ from moveit_configs_utils.launches import generate_rsp_launch
 
 
 def generate_launch_description():
-    moveit_config = MoveItConfigsBuilder("ur3e_robotiq", package_name="ur_robotiq_moveit_config").to_moveit_configs()
+    moveit_config = MoveItConfigsBuilder("ur_robotiq", package_name="ur_robotiq_moveit_config").to_moveit_configs()
     return generate_rsp_launch(moveit_config)
diff --git a/src/ur_robotiq_moveit_config/launch/setup_assistant.launch.py b/src/ur_robotiq_moveit_config/launch/setup_assistant.launch.py
deleted file mode 100644
index 5c1f832..0000000
--- a/src/ur_robotiq_moveit_config/launch/setup_assistant.launch.py
+++ /dev/null
@@ -1,7 +0,0 @@
-from moveit_configs_utils import MoveItConfigsBuilder
-from moveit_configs_utils.launches import generate_setup_assistant_launch
-
-
-def generate_launch_description():
-    moveit_config = MoveItConfigsBuilder("ur3e_robotiq", package_name="ur_robotiq_moveit_config").to_moveit_configs()
-    return generate_setup_assistant_launch(moveit_config)
diff --git a/src/ur_robotiq_moveit_config/launch/spawn_controllers.launch.py b/src/ur_robotiq_moveit_config/launch/spawn_controllers.launch.py
index e716754..405a2f3 100644
--- a/src/ur_robotiq_moveit_config/launch/spawn_controllers.launch.py
+++ b/src/ur_robotiq_moveit_config/launch/spawn_controllers.launch.py
@@ -3,5 +3,5 @@ from moveit_configs_utils.launches import generate_spawn_controllers_launch
 
 
 def generate_launch_description():
-    moveit_config = MoveItConfigsBuilder("ur3e_robotiq", package_name="ur_robotiq_moveit_config").to_moveit_configs()
+    moveit_config = MoveItConfigsBuilder("ur_robotiq", package_name="ur_robotiq_moveit_config").to_moveit_configs()
     return generate_spawn_controllers_launch(moveit_config)
diff --git a/src/ur_robotiq_moveit_config/launch/static_virtual_joint_tfs.launch.py b/src/ur_robotiq_moveit_config/launch/static_virtual_joint_tfs.launch.py
index 35f8b7d..3527d48 100644
--- a/src/ur_robotiq_moveit_config/launch/static_virtual_joint_tfs.launch.py
+++ b/src/ur_robotiq_moveit_config/launch/static_virtual_joint_tfs.launch.py
@@ -3,5 +3,5 @@ from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_laun
 
 
 def generate_launch_description():
-    moveit_config = MoveItConfigsBuilder("ur3e_robotiq", package_name="ur_robotiq_moveit_config").to_moveit_configs()
+    moveit_config = MoveItConfigsBuilder("ur_robotiq", package_name="ur_robotiq_moveit_config").to_moveit_configs()
     return generate_static_virtual_joint_tfs_launch(moveit_config)
diff --git a/src/ur_robotiq_moveit_config/launch/ur3e_robotiq.launch.py b/src/ur_robotiq_moveit_config/launch/ur3e_robotiq.launch.py
new file mode 100644
index 0000000..97ad952
--- /dev/null
+++ b/src/ur_robotiq_moveit_config/launch/ur3e_robotiq.launch.py
@@ -0,0 +1,280 @@
+import os
+
+import launch_ros
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import OpaqueFunction
+from launch.conditions import IfCondition
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.parameter_descriptions import ParameterValue
+from ament_index_python.packages import get_package_share_directory
+import yaml
+
+
+def load_yaml(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, 'r') as file:
+            return yaml.safe_load(file)
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def generate_launch_description():
+
+    # Initialize Arguments
+    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
+    # General arguments
+    robot_name = LaunchConfiguration("robot_name")
+    prefix = LaunchConfiguration("prefix")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    launch_rviz = LaunchConfiguration("launch_rviz")
+    launch_servo = LaunchConfiguration("launch_servo")
+    ur_type = "ur3e"
+
+    # File and package names
+    ur_description_package = "ur_description"
+    ur_robotiq_description_package = "ur_robotiq_description"
+    ur_robotiq_description_file = "ur_robotiq.urdf.xacro"
+    moveit_config_package = "ur_robotiq_moveit_config"
+    moveit_config_file = "ur_robotiq_macro.srdf.xacro"
+
+    joint_limit_params = PathJoinSubstitution(
+        [FindPackageShare(ur_description_package), "config", ur_type, "joint_limits.yaml"]
+    )
+    kinematics_params = PathJoinSubstitution(
+        [FindPackageShare(ur_description_package), "config", ur_type, "default_kinematics.yaml"]
+    )
+    physical_params = PathJoinSubstitution(
+        [FindPackageShare(ur_description_package), "config", ur_type, "physical_parameters.yaml"]
+    )
+    visual_params = PathJoinSubstitution(
+        [FindPackageShare(ur_description_package), "config", ur_type, "visual_parameters.yaml"]
+    )
+
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution([FindPackageShare(ur_robotiq_description_package), "urdf", ur_robotiq_description_file]),
+            " ",
+            "robot_ip:=xxx.yyy.zzz.www",
+            " ",
+            "use_fake_hardware:=",
+            use_fake_hardware,
+            " ",
+            "joint_limit_params:=",
+            joint_limit_params,
+            " ",
+            "kinematics_params:=",
+            kinematics_params,
+            " ",
+            "physical_params:=",
+            physical_params,
+            " ",
+            "visual_params:=",
+            visual_params,
+            " ",
+            "safety_limits:=true",
+            " ",
+            "safety_pos_margin:=0.15",
+            " ",
+            "safety_k_position:=20",
+            " ",
+            "name:=",
+            robot_name,
+            " ",
+            "ur_type:=",
+            ur_type,
+            " ",
+            "script_filename:=ros_control.urscript",
+            " ",
+            "input_recipe_filename:=rtde_input_recipe.txt",
+            " ",
+            "output_recipe_filename:=rtde_output_recipe.txt",
+            " ",
+            "prefix:=",
+            prefix,
+            " ",
+        ]
+    )
+
+    robot_description = {"robot_description": robot_description_content}
+
+    # MoveIt Configuration
+    robot_description_semantic_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution([FindPackageShare(moveit_config_package), "srdf", moveit_config_file]),
+            " ",
+            "name:=",
+            robot_name,
+            " ",
+            "prefix:=",
+            prefix,
+            " "
+        ]
+    )
+
+    robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
+
+    robot_description_kinematics = load_yaml(moveit_config_package, "config/kinematics.yaml")
+
+    # robot_description_planning = {
+    # "robot_description_planning": load_yaml_abs(str(joint_limit_params.perform(context)))
+    # }
+
+    # Planning Configuration
+    ompl_planning_pipeline_config = {
+        "move_group": {
+            "planning_plugin": "ompl_interface/OMPLPlanner",
+            "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
+            "start_state_max_bounds_error": 0.1,
+        }
+    }
+
+    ompl_planning_yaml = load_yaml(moveit_config_package, "config/ompl_planning.yaml")
+    ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
+
+    ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
+
+    # Trajectory Execution Configuration
+    controllers_yaml = load_yaml(moveit_config_package, "config/controllers.yaml")
+    # the scaled_joint_trajectory_controller does not work on fake hardware
+    change_controllers = use_fake_hardware
+    if change_controllers == "true":
+        controllers_yaml["scaled_joint_trajectory_controller"]["default"] = False
+        controllers_yaml["joint_trajectory_controller"]["default"] = True
+
+    moveit_controllers = {
+        "moveit_simple_controller_manager": controllers_yaml,
+        "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
+    }
+
+    trajectory_execution = {
+        "moveit_manage_controllers": False,
+        "trajectory_execution.allowed_execution_duration_scaling": 1.2,
+        "trajectory_execution.allowed_goal_duration_margin": 0.5,
+        "trajectory_execution.allowed_start_tolerance": 0.01,
+    }
+
+    planning_scene_monitor_parameters = {
+        "publish_planning_scene": True,
+        "publish_geometry_updates": True,
+        "publish_state_updates": True,
+        "publish_transforms_updates": True,
+    }
+    print(robot_description,
+          robot_description_semantic,
+          robot_description_kinematics,
+          ompl_planning_pipeline_config,
+          trajectory_execution,
+          moveit_controllers,
+          planning_scene_monitor_parameters)
+    # Start the actual move_group node/action server
+    move_group_node = Node(
+        package="moveit_ros_move_group",
+        executable="move_group",
+        output="screen",
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            robot_description_kinematics,
+            # robot_description_planning,
+            ompl_planning_pipeline_config,
+            trajectory_execution,
+            moveit_controllers,
+            planning_scene_monitor_parameters,
+            {"use_sim_time": use_sim_time},
+        ],
+    )
+
+    robot_state_publisher_node = launch_ros.actions.Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        parameters=[robot_description],
+    )
+
+    # rviz with moveit configuration
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare(moveit_config_package), "rviz", "view_robot.rviz"]
+    )
+    rviz_node = Node(
+        package="rviz2",
+        condition=IfCondition(launch_rviz),
+        executable="rviz2",
+        name="rviz2_moveit",
+        output="log",
+        arguments=["-d", rviz_config_file],
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            ompl_planning_pipeline_config,
+            robot_description_kinematics,
+            # robot_description_planning,
+        ],
+    )
+
+    # Servo node for realtime control
+
+    servo_yaml = load_yaml(moveit_config_package, "config/ur_servo.yaml")
+    servo_params = {"moveit_servo": servo_yaml}
+    # servo_node = Node(
+    #     package="moveit_servo",
+    #     condition=IfCondition(launch_servo),
+    #     executable="servo_node_main",
+    #     parameters=[
+    #         servo_params,
+    #         robot_description,
+    #         robot_description_semantic,
+    #     ],
+    #     output="screen",
+    # )
+
+    # Declare the Launch arguments
+    # UR specific arguments
+    robot_name_arg = DeclareLaunchArgument(
+        "robot_name",
+        default_value= "ur",
+        description="Name of the Universal Robot"
+    )
+
+    use_fake_hardware_arg = DeclareLaunchArgument(
+            "use_fake_hardware",
+            default_value="true",
+            description="Indicate whether robot is running with fake hardware mirroring command to its states.",
+        )
+
+    # General arguments
+    use_sim_time_arg = DeclareLaunchArgument(
+        "use_sim_time",
+        default_value="false",
+        description="Make MoveIt to use simulation time. This is needed for the trajectory planing in simulation.",
+    )
+    prefix_arg = DeclareLaunchArgument(
+        "prefix",
+        default_value='""',
+        description="Prefix of the joint names, useful for \
+    multi-robot setup. If changed than also joint names in the controllers' configuration \
+    have to be updated.",
+    )
+    launch_rviz_arg = DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")
+    launch_servo_arg = DeclareLaunchArgument("launch_servo", default_value="true", description="Launch Servo?")
+
+    launch_args = [
+        robot_name_arg,
+        prefix_arg,
+        use_fake_hardware_arg,
+        use_sim_time_arg,
+        launch_rviz_arg,
+        launch_servo_arg
+    ]
+    # servo_node,
+    nodes_to_start = [move_group_node, rviz_node,  robot_state_publisher_node]
+
+    return LaunchDescription(launch_args + nodes_to_start)
diff --git a/src/ur_robotiq_moveit_config/launch/ur3e_robotiq_description.launch.py b/src/ur_robotiq_moveit_config/launch/ur3e_robotiq_description.launch.py
deleted file mode 100644
index c423602..0000000
--- a/src/ur_robotiq_moveit_config/launch/ur3e_robotiq_description.launch.py
+++ /dev/null
@@ -1,40 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-import os
-from ament_index_python.packages import get_package_share_directory
-import xacro
-
-def generate_launch_description():
-
-    # Define the path to the Xacro file
-    ur_robotiq_desc_pkg_path = get_package_share_directory('ur_robotiq_description')
-    xacro_file_path = os.path.join(ur_robotiq_desc_pkg_path, 'urdf', 'ur_robotiq.urdf.xacro')
-
-    # Process the Xacro file to generate URDF
-    doc = xacro.process_file(xacro_file_path)
-    robot_description = {'robot_description': doc.toxml()}
-
-    # Node to publish robot state to TF
-    robot_state_publisher_node = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        output='both',
-        parameters=[robot_description],
-    )
-
-    # Node to launch RViz
-    rviz_node = Node(
-        package='rviz2',
-        executable='rviz2',
-        name='rviz2',
-        arguments=['-d', os.path.join(ur_robotiq_desc_pkg_path, 'rviz', 'ur3e_robotiq.rviz')],
-        output='screen'
-    )
-
-    # Define and return LaunchDescription
-    return LaunchDescription([
-        robot_state_publisher_node,
-        rviz_node,
-        # Add other necessary nodes and configurations here
-    ])
-
diff --git a/src/ur_robotiq_moveit_config/launch/warehouse_db.launch.py b/src/ur_robotiq_moveit_config/launch/warehouse_db.launch.py
deleted file mode 100644
index 55ea6df..0000000
--- a/src/ur_robotiq_moveit_config/launch/warehouse_db.launch.py
+++ /dev/null
@@ -1,7 +0,0 @@
-from moveit_configs_utils import MoveItConfigsBuilder
-from moveit_configs_utils.launches import generate_warehouse_db_launch
-
-
-def generate_launch_description():
-    moveit_config = MoveItConfigsBuilder("ur3e_robotiq", package_name="ur_robotiq_moveit_config").to_moveit_configs()
-    return generate_warehouse_db_launch(moveit_config)
diff --git a/src/ur_robotiq_moveit_config/package.xml b/src/ur_robotiq_moveit_config/package.xml
index 5a075b7..0725496 100644
--- a/src/ur_robotiq_moveit_config/package.xml
+++ b/src/ur_robotiq_moveit_config/package.xml
@@ -4,9 +4,9 @@
   <name>ur_robotiq_moveit_config</name>
   <version>0.3.0</version>
   <description>
-     An automatically generated package with all the configuration and launch files for using the ur3e_robotiq with the MoveIt Motion Planning Framework
+     An automatically generated package with all the configuration and launch files for using the ur_robotiq with the MoveIt Motion Planning Framework
   </description>
-  <maintainer email="fotios.lygerakis@unileoben.ac.at">Fotios Lygerakis</maintainer>
+  <maintainer email="nikolaus.feith@unileoben.ac.at">Nikolaus Feith</maintainer>
 
   <license>BSD</license>
 
@@ -14,7 +14,7 @@
   <url type="bugtracker">https://github.com/ros-planning/moveit2/issues</url>
   <url type="repository">https://github.com/ros-planning/moveit2</url>
 
-  <author email="fotios.lygerakis@unileoben.ac.at">Fotios Lygerakis</author>
+  <author email="nikolaus.feith@unileoben.ac.at">Nikolaus Feith</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
@@ -34,17 +34,13 @@
   <exec_depend>moveit_configs_utils</exec_depend>
   <exec_depend>moveit_ros_move_group</exec_depend>
   <exec_depend>moveit_ros_visualization</exec_depend>
-  <exec_depend>moveit_ros_warehouse</exec_depend>
-  <exec_depend>moveit_setup_assistant</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>rviz2</exec_depend>
   <exec_depend>rviz_common</exec_depend>
   <exec_depend>rviz_default_plugins</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
   <exec_depend>ur_robotiq_description</exec_depend>
-  <exec_depend>warehouse_ros_mongo</exec_depend>
   <exec_depend>xacro</exec_depend>
-  <exec_depend>ament_index_python</exec_depend>
 
 
   <export>
diff --git a/src/ur_robotiq_moveit_config/config/moveit.rviz b/src/ur_robotiq_moveit_config/rviz/moveit.rviz
similarity index 100%
rename from src/ur_robotiq_moveit_config/config/moveit.rviz
rename to src/ur_robotiq_moveit_config/rviz/moveit.rviz
diff --git a/src/ur_robotiq_moveit_config/rviz/view_robot.rviz b/src/ur_robotiq_moveit_config/rviz/view_robot.rviz
new file mode 100644
index 0000000..7a4b6ff
--- /dev/null
+++ b/src/ur_robotiq_moveit_config/rviz/view_robot.rviz
@@ -0,0 +1,285 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+        - /RobotModel1/Status1
+        - /RobotModel1/Links1
+      Splitter Ratio: 0.5
+    Tree Height: 746
+  - 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
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: 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
+        ft_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        left_inner_finger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_inner_finger_pad:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_inner_knuckle:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_outer_finger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_outer_knuckle:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_inner_finger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_inner_finger_pad:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_inner_knuckle:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_outer_finger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_outer_knuckle:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        robotiq_140_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        robotiq_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        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
+        ur_to_robotiq_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        world:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        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
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  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
+      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: 1.6701574325561523
+      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:
+  Displays:
+    collapsed: false
+  Height: 1043
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000002a000000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003c50000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1920
+  X: 0
+  Y: 0
diff --git a/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro b/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro
new file mode 100644
index 0000000..9414a7e
--- /dev/null
+++ b/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro
@@ -0,0 +1,77 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro">
+  <xacro:macro name="ur_robotiq_srdf" params="name prefix">
+    <!--GROUPS - Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+    <!--LINKS - When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+    <!--JOINTS - When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+    <!--CHAINS - When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+    <!--SUBGROUPS - Groups can also be formed by referencing to already defined group names-->
+    <group name="${prefix}${name}_manipulator">
+      <chain base_link="${prefix}base_link" tip_link="${prefix}tool0" />
+    </group>
+    <group name="${prefix}gripper">
+        <link name="${prefix}robotiq_base_link"/>
+        <link name="${prefix}left_inner_knuckle"/>
+    </group>
+    <!--GROUP STATES - Purpose - Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
+    <group_state name="${prefix}home" group="${prefix}${name}_manipulator">
+      <joint name="${prefix}elbow_joint" value="0" />
+      <joint name="${prefix}shoulder_lift_joint" value="-1.5707" />
+      <joint name="${prefix}shoulder_pan_joint" value="0" />
+      <joint name="${prefix}wrist_1_joint" value="0" />
+      <joint name="${prefix}wrist_2_joint" value="0" />
+      <joint name="${prefix}wrist_3_joint" value="0" />
+    </group_state>
+    <group_state name="${prefix}up" group="${prefix}${name}_manipulator">
+      <joint name="${prefix}elbow_joint" value="0" />
+      <joint name="${prefix}shoulder_lift_joint" value="-1.5707" />
+      <joint name="${prefix}shoulder_pan_joint" value="0" />
+      <joint name="${prefix}wrist_1_joint" value="-1.5707" />
+      <joint name="${prefix}wrist_2_joint" value="0" />
+      <joint name="${prefix}wrist_3_joint" value="0" />
+    </group_state>
+    <group_state name="${prefix}test_configuration" group="${prefix}${name}_manipulator">
+      <joint name="${prefix}elbow_joint" value="1.4" />
+      <joint name="${prefix}shoulder_lift_joint" value="-1.62" />
+      <joint name="${prefix}shoulder_pan_joint" value="1.54" />
+      <joint name="${prefix}wrist_1_joint" value="-1.2" />
+      <joint name="${prefix}wrist_2_joint" value="-1.6" />
+      <joint name="${prefix}wrist_3_joint" value="-0.11" />
+    </group_state>
+    <!--END EFFECTOR - Purpose - Represent information about an end effector.-->
+    <!--VIRTUAL JOINT - Purpose - this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
+    <!--DISABLE COLLISIONS - By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+    <!--UR-Robot-->
+    <disable_collisions link1="${prefix}base_link" link2="${prefix}base_link_inertia" reason="Adjacent" />
+    <disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}shoulder_link" reason="Adjacent" />
+    <disable_collisions link1="${prefix}tool0" link2="${prefix}wrist_1_link" reason="Never" />
+    <disable_collisions link1="${prefix}tool0" link2="${prefix}wrist_2_link" reason="Never" />
+    <disable_collisions link1="${prefix}tool0" link2="${prefix}wrist_3_link" reason="Adjacent" />
+    <disable_collisions link1="${prefix}forearm_link" link2="${prefix}upper_arm_link" reason="Adjacent" />
+    <disable_collisions link1="${prefix}forearm_link" link2="${prefix}wrist_1_link" reason="Adjacent" />
+    <disable_collisions link1="${prefix}shoulder_link" link2="${prefix}upper_arm_link" reason="Adjacent" />
+    <disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_2_link" reason="Adjacent" />
+    <disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_3_link" reason="Never" />
+    <disable_collisions link1="${prefix}wrist_2_link" link2="${prefix}wrist_3_link" reason="Adjacent" />
+    <!--Robotiq-Gripper-->
+    <disable_collisions link1="${prefix}tool0" link2="${prefix}robotiq_base_link" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_base_link" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="left_outer_knuckle" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}left_outer_finger" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}left_inner_knuckle" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}left_outer_finger" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}left_inner_knuckle" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}left_inner_finger" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}left_inner_finger" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_inner_finger_pad" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="right_outer_knuckle" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}right_outer_finger" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}right_inner_knuckle" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}right_inner_knuckle" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}right_outer_finger" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}right_inner_finger" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}right_inner_finger" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_inner_finger_pad" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}right_inner_finger_pad" reason="Never"/>
+  </xacro:macro>
+</robot>
\ No newline at end of file