From 33f69894ebb1e8b6dbacef58bfc782768d0b39d1 Mon Sep 17 00:00:00 2001
From: Felix Durchdewald <Durchdewald@fzi.de>
Date: Thu, 19 Oct 2023 12:24:02 +0200
Subject: [PATCH 1/7] removed use_mock_hardware launch argument

---
 ur_moveit_config/launch/ur_moveit.launch.py | 13 -------------
 1 file changed, 13 deletions(-)

diff --git a/ur_moveit_config/launch/ur_moveit.launch.py b/ur_moveit_config/launch/ur_moveit.launch.py
index 256dc1aed..d1aaa8860 100644
--- a/ur_moveit_config/launch/ur_moveit.launch.py
+++ b/ur_moveit_config/launch/ur_moveit.launch.py
@@ -44,7 +44,6 @@
 def launch_setup(context, *args, **kwargs):
     # Initialize Arguments
     ur_type = LaunchConfiguration("ur_type")
-    use_mock_hardware = LaunchConfiguration("use_mock_hardware")
     safety_limits = LaunchConfiguration("safety_limits")
     safety_pos_margin = LaunchConfiguration("safety_pos_margin")
     safety_k_position = LaunchConfiguration("safety_k_position")
@@ -166,11 +165,6 @@ def launch_setup(context, *args, **kwargs):
 
     # Trajectory Execution Configuration
     controllers_yaml = load_yaml("ur_moveit_config", "config/controllers.yaml")
-    # the scaled_joint_trajectory_controller does not work on mock hardware
-    change_controllers = context.perform_substitution(use_mock_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,
@@ -266,13 +260,6 @@ def generate_launch_description():
             choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20"],
         )
     )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "use_mock_hardware",
-            default_value="false",
-            description="Indicate whether robot is running with mock hardware mirroring command to its states.",
-        )
-    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "safety_limits",

From 919c525f7143a70a9bbcd728797430a1302eec4f Mon Sep 17 00:00:00 2001
From: Felix Durchdewald <Durchdewald@fzi.de>
Date: Thu, 19 Oct 2023 12:29:23 +0200
Subject: [PATCH 2/7] removed jtc from moveit controllers file

---
 ur_moveit_config/config/controllers.yaml | 15 ---------------
 1 file changed, 15 deletions(-)

diff --git a/ur_moveit_config/config/controllers.yaml b/ur_moveit_config/config/controllers.yaml
index 784ebd315..07683929a 100644
--- a/ur_moveit_config/config/controllers.yaml
+++ b/ur_moveit_config/config/controllers.yaml
@@ -1,7 +1,5 @@
 controller_names:
   - scaled_joint_trajectory_controller
-  - joint_trajectory_controller
-
 
 scaled_joint_trajectory_controller:
   action_ns: follow_joint_trajectory
@@ -14,16 +12,3 @@ scaled_joint_trajectory_controller:
     - 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

From baeb0b41786669bb3ebd8930bd20e4bf051a0056 Mon Sep 17 00:00:00 2001
From: Felix Durchdewald <Durchdewald@fzi.de>
Date: Thu, 19 Oct 2023 12:44:07 +0200
Subject: [PATCH 3/7] changed documentation to match moveit launchfile without
 use_mock_hardware argument

---
 ur_robot_driver/doc/usage.rst | 14 +++++++++-----
 1 file changed, 9 insertions(+), 5 deletions(-)

diff --git a/ur_robot_driver/doc/usage.rst b/ur_robot_driver/doc/usage.rst
index 48c59c745..37760cb70 100644
--- a/ur_robot_driver/doc/usage.rst
+++ b/ur_robot_driver/doc/usage.rst
@@ -73,7 +73,7 @@ outside of this driver's scope.
    * - mode
      - available controllers
    * - mock_hardware
-     - :raw-html-m2r:`<ul><li>joint_trajectory_controller</li><li>forward_velocity_controller</li><li>forward_position_controller</li></ul>`
+     - :raw-html-m2r:`<ul><li>joint_trajectory_controller</li><li>scaled_joint_trajectory_controller </li><li>forward_velocity_controller</li><li>forward_position_controller</li></ul>`
    * - real hardware / URSim
      - :raw-html-m2r:`<ul><li>joint_trajectory_controller</li><li>scaled_joint_trajectory_controller </li><li>forward_velocity_controller</li><li>forward_position_controller</li></ul>`
 
@@ -194,14 +194,18 @@ robot as explained `here <https://moveit.picknik.ai/galactic/doc/tutorials/quick
 Mock hardware
 ^^^^^^^^^^^^^
 
-Currently, the ``scaled_joint_trajectory_controller`` does not work with ros2_control mock_hardware. There is an
-`upstream Merge-Request <https://github.com/ros-controls/ros2_control/pull/822>`_ pending to fix that. Until this is merged and released, you'll have to fallback to the ``joint_trajectory_controller`` by passing ``initial_controller:=joint_trajectory_controller`` to the driver's startup. Also, you'll have to tell MoveIt! that you're using mock_hardware as it then has to map to the other controller:
+To test the driver on mock hardware with the example MoveIt-setup, first start the driver as described
+`above <#start-hardware-simulator-or-mockup>`_.
 
 .. code-block::
 
-   ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=yyy.yyy.yyy.yyy use_mock_hardware:=true launch_rviz:=false initial_joint_controller:=joint_trajectory_controller
+   ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=yyy.yyy.yyy.yyy use_mock_hardware:=true launch_rviz:=false
    # and in another shell
-   ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=ur5e launch_rviz:=true use_mock_hardware:=true
+   ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=ur5e launch_rviz:=true
+
+Now you should be able to use the MoveIt Plugin in rviz2 to plan and execute trajectories with the
+robot as explained `here <https://moveit.picknik.ai/galactic/doc/tutorials/quickstart_in_rviz/quickstart_in_rviz_tutorial.html>`_.
+
 
 Robot frames
 ------------

From 45c53971d415d451bdede28de36acf2a52e0bd77 Mon Sep 17 00:00:00 2001
From: "Felix Exner (fexner)" <felix_mauch@web.de>
Date: Wed, 15 Nov 2023 16:24:09 +0100
Subject: [PATCH 4/7] Update ur_robot_driver/doc/usage.rst

---
 ur_robot_driver/doc/usage.rst | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/ur_robot_driver/doc/usage.rst b/ur_robot_driver/doc/usage.rst
index 37760cb70..fa02d8c68 100644
--- a/ur_robot_driver/doc/usage.rst
+++ b/ur_robot_driver/doc/usage.rst
@@ -204,7 +204,7 @@ To test the driver on mock hardware with the example MoveIt-setup, first start t
    ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=ur5e launch_rviz:=true
 
 Now you should be able to use the MoveIt Plugin in rviz2 to plan and execute trajectories with the
-robot as explained `here <https://moveit.picknik.ai/galactic/doc/tutorials/quickstart_in_rviz/quickstart_in_rviz_tutorial.html>`_.
+robot as explained `in the MoveIt! documentation <https://moveit.picknik.ai/galactic/doc/tutorials/quickstart_in_rviz/quickstart_in_rviz_tutorial.html>`_.
 
 
 Robot frames

From 7b4c96870abbc5952e313d486684e063639f3358 Mon Sep 17 00:00:00 2001
From: Felix Durchdewald <Durchdewald@fzi.de>
Date: Wed, 29 Nov 2023 17:23:12 +0100
Subject: [PATCH 5/7] take robot_description directly from topic

---
 ur_moveit_config/launch/ur_moveit.launch.py | 77 +--------------------
 1 file changed, 3 insertions(+), 74 deletions(-)

diff --git a/ur_moveit_config/launch/ur_moveit.launch.py b/ur_moveit_config/launch/ur_moveit.launch.py
index d1aaa8860..75766882a 100644
--- a/ur_moveit_config/launch/ur_moveit.launch.py
+++ b/ur_moveit_config/launch/ur_moveit.launch.py
@@ -44,12 +44,7 @@
 def launch_setup(context, *args, **kwargs):
     # Initialize Arguments
     ur_type = LaunchConfiguration("ur_type")
-    safety_limits = LaunchConfiguration("safety_limits")
-    safety_pos_margin = LaunchConfiguration("safety_pos_margin")
-    safety_k_position = LaunchConfiguration("safety_k_position")
     # General arguments
-    description_package = LaunchConfiguration("description_package")
-    description_file = LaunchConfiguration("description_file")
     moveit_config_package = LaunchConfiguration("moveit_config_package")
     moveit_joint_limits_file = LaunchConfiguration("moveit_joint_limits_file")
     moveit_config_file = LaunchConfiguration("moveit_config_file")
@@ -59,67 +54,6 @@ def launch_setup(context, *args, **kwargs):
     launch_rviz = LaunchConfiguration("launch_rviz")
     launch_servo = LaunchConfiguration("launch_servo")
 
-    joint_limit_params = PathJoinSubstitution(
-        [FindPackageShare(description_package), "config", ur_type, "joint_limits.yaml"]
-    )
-    kinematics_params = PathJoinSubstitution(
-        [FindPackageShare(description_package), "config", ur_type, "default_kinematics.yaml"]
-    )
-    physical_params = PathJoinSubstitution(
-        [FindPackageShare(description_package), "config", ur_type, "physical_parameters.yaml"]
-    )
-    visual_params = PathJoinSubstitution(
-        [FindPackageShare(description_package), "config", ur_type, "visual_parameters.yaml"]
-    )
-
-    robot_description_content = Command(
-        [
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
-            " ",
-            PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]),
-            " ",
-            "robot_ip:=xxx.yyy.zzz.www",
-            " ",
-            "joint_limit_params:=",
-            joint_limit_params,
-            " ",
-            "kinematics_params:=",
-            kinematics_params,
-            " ",
-            "physical_params:=",
-            physical_params,
-            " ",
-            "visual_params:=",
-            visual_params,
-            " ",
-            "safety_limits:=",
-            safety_limits,
-            " ",
-            "safety_pos_margin:=",
-            safety_pos_margin,
-            " ",
-            "safety_k_position:=",
-            safety_k_position,
-            " ",
-            "name:=",
-            "ur",
-            " ",
-            "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(
         [
@@ -130,9 +64,7 @@ def launch_setup(context, *args, **kwargs):
             ),
             " ",
             "name:=",
-            # Also ur_type parameter could be used but then the planning group names in yaml
-            # configs has to be updated!
-            "ur",
+            ur_type,
             " ",
             "prefix:=",
             prefix,
@@ -196,7 +128,6 @@ def launch_setup(context, *args, **kwargs):
         executable="move_group",
         output="screen",
         parameters=[
-            robot_description,
             robot_description_semantic,
             robot_description_kinematics,
             robot_description_planning,
@@ -221,7 +152,6 @@ def launch_setup(context, *args, **kwargs):
         output="log",
         arguments=["-d", rviz_config_file],
         parameters=[
-            robot_description,
             robot_description_semantic,
             ompl_planning_pipeline_config,
             robot_description_kinematics,
@@ -236,10 +166,9 @@ def launch_setup(context, *args, **kwargs):
     servo_node = Node(
         package="moveit_servo",
         condition=IfCondition(launch_servo),
-        executable="servo_node_main",
+        executable="servo_node",
         parameters=[
             servo_params,
-            robot_description,
             robot_description_semantic,
         ],
         output="screen",
@@ -252,7 +181,7 @@ def launch_setup(context, *args, **kwargs):
 
 def generate_launch_description():
     declared_arguments = []
-    # UR specific arguments
+    # UR specific arguments.
     declared_arguments.append(
         DeclareLaunchArgument(
             "ur_type",

From 1ae71fa16588e4b6ec58c735b401cb5c701cb483 Mon Sep 17 00:00:00 2001
From: Felix Durchdewald <Durchdewald@fzi.de>
Date: Wed, 29 Nov 2023 17:25:06 +0100
Subject: [PATCH 6/7] move_group name set to ur_manipulator for every ur_type

---
 ur_moveit_config/launch/ur_moveit.launch.py |  2 +-
 ur_moveit_config/srdf/ur.srdf.xacro         |  4 +---
 ur_moveit_config/srdf/ur_macro.srdf.xacro   | 10 +++++-----
 3 files changed, 7 insertions(+), 9 deletions(-)

diff --git a/ur_moveit_config/launch/ur_moveit.launch.py b/ur_moveit_config/launch/ur_moveit.launch.py
index 75766882a..f6ac74a1c 100644
--- a/ur_moveit_config/launch/ur_moveit.launch.py
+++ b/ur_moveit_config/launch/ur_moveit.launch.py
@@ -166,7 +166,7 @@ def launch_setup(context, *args, **kwargs):
     servo_node = Node(
         package="moveit_servo",
         condition=IfCondition(launch_servo),
-        executable="servo_node",
+        executable="servo_node_main",
         parameters=[
             servo_params,
             robot_description_semantic,
diff --git a/ur_moveit_config/srdf/ur.srdf.xacro b/ur_moveit_config/srdf/ur.srdf.xacro
index 0ae2cca27..80bcedddf 100644
--- a/ur_moveit_config/srdf/ur.srdf.xacro
+++ b/ur_moveit_config/srdf/ur.srdf.xacro
@@ -1,13 +1,11 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <robot xmlns:xacro="http://wiki.ros.org/xacro" name="$(arg name)">
 
-  <!-- robot name parameter -->
-  <xacro:arg name="name" default="ur"/>
   <!-- parameters -->
   <xacro:arg name="prefix" default="" />
 
   <xacro:include filename="$(find ur_moveit_config)/srdf/ur_macro.srdf.xacro"/>
 
-  <xacro:ur_srdf name="$(arg name)" prefix="$(arg prefix)"/>
+  <xacro:ur_srdf prefix="$(arg prefix)"/>
 
 </robot>
diff --git a/ur_moveit_config/srdf/ur_macro.srdf.xacro b/ur_moveit_config/srdf/ur_macro.srdf.xacro
index 04554f33c..8c2f319b9 100644
--- a/ur_moveit_config/srdf/ur_macro.srdf.xacro
+++ b/ur_moveit_config/srdf/ur_macro.srdf.xacro
@@ -4,17 +4,17 @@
     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
 -->
-  <xacro:macro name="ur_srdf" params="name prefix">
+  <xacro:macro name="ur_srdf" params="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">
+    <group name="${prefix}ur_manipulator">
       <chain base_link="${prefix}base_link" tip_link="${prefix}tool0" />
     </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">
+    <group_state name="${prefix}home" group="${prefix}ur_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" />
@@ -22,7 +22,7 @@
       <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">
+    <group_state name="${prefix}up" group="${prefix}ur_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" />
@@ -30,7 +30,7 @@
       <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">
+    <group_state name="${prefix}test_configuration" group="${prefix}ur_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" />

From 65f97a1ba604f0eb5460215deffc5033b2e88604 Mon Sep 17 00:00:00 2001
From: Felix Durchdewald <Durchdewald@fzi.de>
Date: Mon, 18 Dec 2023 14:58:12 +0100
Subject: [PATCH 7/7] added option to change moveit controller

---
 ur_moveit_config/config/controllers.yaml    | 16 +++++++++++++++-
 ur_moveit_config/launch/ur_moveit.launch.py | 13 +++++++++++--
 2 files changed, 26 insertions(+), 3 deletions(-)

diff --git a/ur_moveit_config/config/controllers.yaml b/ur_moveit_config/config/controllers.yaml
index 07683929a..afafbb68a 100644
--- a/ur_moveit_config/config/controllers.yaml
+++ b/ur_moveit_config/config/controllers.yaml
@@ -1,10 +1,24 @@
 controller_names:
   - scaled_joint_trajectory_controller
+  - joint_trajectory_controller
 
 scaled_joint_trajectory_controller:
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
-  default: true
+  default: false
+  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
diff --git a/ur_moveit_config/launch/ur_moveit.launch.py b/ur_moveit_config/launch/ur_moveit.launch.py
index f6ac74a1c..b401a6f0d 100644
--- a/ur_moveit_config/launch/ur_moveit.launch.py
+++ b/ur_moveit_config/launch/ur_moveit.launch.py
@@ -44,6 +44,7 @@
 def launch_setup(context, *args, **kwargs):
     # Initialize Arguments
     ur_type = LaunchConfiguration("ur_type")
+    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
     # General arguments
     moveit_config_package = LaunchConfiguration("moveit_config_package")
     moveit_joint_limits_file = LaunchConfiguration("moveit_joint_limits_file")
@@ -53,7 +54,6 @@ def launch_setup(context, *args, **kwargs):
     use_sim_time = LaunchConfiguration("use_sim_time")
     launch_rviz = LaunchConfiguration("launch_rviz")
     launch_servo = LaunchConfiguration("launch_servo")
-
     # MoveIt Configuration
     robot_description_semantic_content = Command(
         [
@@ -97,6 +97,8 @@ def launch_setup(context, *args, **kwargs):
 
     # Trajectory Execution Configuration
     controllers_yaml = load_yaml("ur_moveit_config", "config/controllers.yaml")
+    initial_controller = context.perform_substitution(initial_joint_controller)
+    controllers_yaml[initial_controller]["default"] = True
 
     moveit_controllers = {
         "moveit_simple_controller_manager": controllers_yaml,
@@ -109,7 +111,6 @@ def launch_setup(context, *args, **kwargs):
         "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,
@@ -189,6 +190,14 @@ def generate_launch_description():
             choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20"],
         )
     )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "initial_joint_controller",
+            default_value="scaled_joint_trajectory_controller",
+            description="Initial controller used by MoveIt!.",
+            choices=["scaled_joint_trajectory_controller", "joint_trajectory_controller"],
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "safety_limits",