From fc46a8d03f22eba402030b4f6810fc2ed7714b86 Mon Sep 17 00:00:00 2001
From: mhubii <m.huber_1994@hotmail.de>
Date: Sat, 29 Jul 2023 15:30:10 +0100
Subject: [PATCH] added med14

---
 .../med14_moveit_config/.setup_assistant      | 12 ++++----
 .../config/initial_positions.yaml             | 10 -------
 .../config/joint_limits.yaml                  | 28 +++++++++----------
 .../config/med14.ros2_control.xacro           | 28 -------------------
 .../med14_moveit_config/config/med14.srdf     |  2 +-
 .../config/med14.urdf.xacro                   | 14 ----------
 .../config/moveit_controllers.yaml            | 20 +++++++++++++
 .../med14_moveit_config/launch/demo.launch.py |  7 -----
 .../launch/move_group.launch.py               | 14 +++++++++-
 .../med14_moveit_config/launch/rsp.launch.py  |  7 -----
 .../launch/spawn_controllers.launch.py        |  7 -----
 .../launch/static_virtual_joint_tfs.launch.py |  7 -----
 .../launch/warehouse_db.launch.py             |  7 -----
 .../med14_moveit_config/package.xml           |  5 ----
 14 files changed, 55 insertions(+), 113 deletions(-)
 delete mode 100644 lbr_moveit_config/med14_moveit_config/config/initial_positions.yaml
 delete mode 100644 lbr_moveit_config/med14_moveit_config/config/med14.ros2_control.xacro
 delete mode 100644 lbr_moveit_config/med14_moveit_config/config/med14.urdf.xacro
 create mode 100644 lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml
 delete mode 100644 lbr_moveit_config/med14_moveit_config/launch/demo.launch.py
 delete mode 100644 lbr_moveit_config/med14_moveit_config/launch/rsp.launch.py
 delete mode 100644 lbr_moveit_config/med14_moveit_config/launch/spawn_controllers.launch.py
 delete mode 100644 lbr_moveit_config/med14_moveit_config/launch/static_virtual_joint_tfs.launch.py
 delete mode 100644 lbr_moveit_config/med14_moveit_config/launch/warehouse_db.launch.py

diff --git a/lbr_moveit_config/med14_moveit_config/.setup_assistant b/lbr_moveit_config/med14_moveit_config/.setup_assistant
index 6670a826..63f8dcc9 100644
--- a/lbr_moveit_config/med14_moveit_config/.setup_assistant
+++ b/lbr_moveit_config/med14_moveit_config/.setup_assistant
@@ -7,17 +7,19 @@ moveit_setup_assistant_config:
   package_settings:
     author_name: mhubii
     author_email: martin.huber@kcl.ac.uk
-    generated_timestamp: 1686315597
+    generated_timestamp: 1690640936
   control_xacro:
     command:
-      []
+      - position
     state:
-      []
+      - position
+      - velocity
   modified_urdf:
     xacros:
       - control_xacro
   control_xacro:
     command:
-      []
+      - position
     state:
-      []
\ No newline at end of file
+      - position
+      - velocity
\ No newline at end of file
diff --git a/lbr_moveit_config/med14_moveit_config/config/initial_positions.yaml b/lbr_moveit_config/med14_moveit_config/config/initial_positions.yaml
deleted file mode 100644
index bbac20fc..00000000
--- a/lbr_moveit_config/med14_moveit_config/config/initial_positions.yaml
+++ /dev/null
@@ -1,10 +0,0 @@
-# Default initial positions for med14's ros2_control fake system
-
-initial_positions:
-  lbr_joint_0: 0
-  lbr_joint_1: 0
-  lbr_joint_2: 0
-  lbr_joint_3: 0
-  lbr_joint_4: 0
-  lbr_joint_5: 0
-  lbr_joint_6: 0
\ No newline at end of file
diff --git a/lbr_moveit_config/med14_moveit_config/config/joint_limits.yaml b/lbr_moveit_config/med14_moveit_config/config/joint_limits.yaml
index 713d6b10..737d4310 100644
--- a/lbr_moveit_config/med14_moveit_config/config/joint_limits.yaml
+++ b/lbr_moveit_config/med14_moveit_config/config/joint_limits.yaml
@@ -11,35 +11,35 @@ joint_limits:
   lbr_joint_0:
     has_velocity_limits: true
     max_velocity: 1.4835298641951802
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
   lbr_joint_1:
     has_velocity_limits: true
     max_velocity: 1.4835298641951802
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
   lbr_joint_2:
     has_velocity_limits: true
     max_velocity: 1.7453292519943295
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
   lbr_joint_3:
     has_velocity_limits: true
     max_velocity: 1.3089969389957472
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
   lbr_joint_4:
     has_velocity_limits: true
     max_velocity: 2.2689280275926285
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
   lbr_joint_5:
     has_velocity_limits: true
     max_velocity: 2.3561944901923448
-    has_acceleration_limits: false
-    max_acceleration: 0
+    has_acceleration_limits: true
+    max_acceleration: 10.0
   lbr_joint_6:
     has_velocity_limits: true
     max_velocity: 2.3561944901923448
-    has_acceleration_limits: false
-    max_acceleration: 0
\ No newline at end of file
+    has_acceleration_limits: true
+    max_acceleration: 10.0
\ No newline at end of file
diff --git a/lbr_moveit_config/med14_moveit_config/config/med14.ros2_control.xacro b/lbr_moveit_config/med14_moveit_config/config/med14.ros2_control.xacro
deleted file mode 100644
index a8fd47ea..00000000
--- a/lbr_moveit_config/med14_moveit_config/config/med14.ros2_control.xacro
+++ /dev/null
@@ -1,28 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-    <xacro:macro name="med14_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="lbr_joint_0">
-            </joint>
-            <joint name="lbr_joint_1">
-            </joint>
-            <joint name="lbr_joint_2">
-            </joint>
-            <joint name="lbr_joint_3">
-            </joint>
-            <joint name="lbr_joint_4">
-            </joint>
-            <joint name="lbr_joint_5">
-            </joint>
-            <joint name="lbr_joint_6">
-            </joint>
-
-        </ros2_control>
-    </xacro:macro>
-</robot>
diff --git a/lbr_moveit_config/med14_moveit_config/config/med14.srdf b/lbr_moveit_config/med14_moveit_config/config/med14.srdf
index c04cac59..0b6f4a3f 100644
--- a/lbr_moveit_config/med14_moveit_config/config/med14.srdf
+++ b/lbr_moveit_config/med14_moveit_config/config/med14.srdf
@@ -13,7 +13,7 @@
         <chain base_link="lbr_link_0" tip_link="lbr_link_ee"/>
     </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="home" group="arm">
+    <group_state name="zero" group="arm">
         <joint name="lbr_joint_0" value="0"/>
         <joint name="lbr_joint_1" value="0"/>
         <joint name="lbr_joint_2" value="0"/>
diff --git a/lbr_moveit_config/med14_moveit_config/config/med14.urdf.xacro b/lbr_moveit_config/med14_moveit_config/config/med14.urdf.xacro
deleted file mode 100644
index 57255477..00000000
--- a/lbr_moveit_config/med14_moveit_config/config/med14.urdf.xacro
+++ /dev/null
@@ -1,14 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="med14">
-    <xacro:arg name="initial_positions_file" default="initial_positions.yaml" />
-
-    <!-- Import med14 urdf file -->
-    <xacro:include filename="$(find lbr_description)/urdf/med14/med14.urdf.xacro" />
-
-    <!-- Import control_xacro -->
-    <xacro:include filename="med14.ros2_control.xacro" />
-
-
-    <xacro:med14_ros2_control name="FakeSystem" initial_positions_file="$(arg initial_positions_file)"/>
-
-</robot>
diff --git a/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml b/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml
new file mode 100644
index 00000000..f10e1498
--- /dev/null
+++ b/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml
@@ -0,0 +1,20 @@
+# MoveIt uses this configuration for controller management
+
+moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
+
+moveit_simple_controller_manager:
+  controller_names:
+    - position_trajectory_controller
+
+  position_trajectory_controller:
+    type: FollowJointTrajectory
+    action_ns: follow_joint_trajectory
+    default: true
+    joints:
+      - lbr_joint_0
+      - lbr_joint_1
+      - lbr_joint_2
+      - lbr_joint_3
+      - lbr_joint_4
+      - lbr_joint_5
+      - lbr_joint_6
\ No newline at end of file
diff --git a/lbr_moveit_config/med14_moveit_config/launch/demo.launch.py b/lbr_moveit_config/med14_moveit_config/launch/demo.launch.py
deleted file mode 100644
index 34ff7239..00000000
--- a/lbr_moveit_config/med14_moveit_config/launch/demo.launch.py
+++ /dev/null
@@ -1,7 +0,0 @@
-from moveit_configs_utils import MoveItConfigsBuilder
-from moveit_configs_utils.launches import generate_demo_launch
-
-
-def generate_launch_description():
-    moveit_config = MoveItConfigsBuilder("med14", package_name="med14_moveit_config").to_moveit_configs()
-    return generate_demo_launch(moveit_config)
diff --git a/lbr_moveit_config/med14_moveit_config/launch/move_group.launch.py b/lbr_moveit_config/med14_moveit_config/launch/move_group.launch.py
index 778294ae..43368b43 100644
--- a/lbr_moveit_config/med14_moveit_config/launch/move_group.launch.py
+++ b/lbr_moveit_config/med14_moveit_config/launch/move_group.launch.py
@@ -1,7 +1,19 @@
+import os
+
+from ament_index_python import get_package_share_directory
 from moveit_configs_utils import MoveItConfigsBuilder
 from moveit_configs_utils.launches import generate_move_group_launch
 
 
 def generate_launch_description():
-    moveit_config = MoveItConfigsBuilder("med14", package_name="med14_moveit_config").to_moveit_configs()
+    moveit_config = (
+        MoveItConfigsBuilder("med14", package_name="med14_moveit_config")
+        .robot_description(
+            os.path.join(
+                get_package_share_directory("lbr_description"),
+                "urdf/med14/med14.urdf.xacro",
+            )
+        )
+        .to_moveit_configs()
+    )
     return generate_move_group_launch(moveit_config)
diff --git a/lbr_moveit_config/med14_moveit_config/launch/rsp.launch.py b/lbr_moveit_config/med14_moveit_config/launch/rsp.launch.py
deleted file mode 100644
index 36eb6b87..00000000
--- a/lbr_moveit_config/med14_moveit_config/launch/rsp.launch.py
+++ /dev/null
@@ -1,7 +0,0 @@
-from moveit_configs_utils import MoveItConfigsBuilder
-from moveit_configs_utils.launches import generate_rsp_launch
-
-
-def generate_launch_description():
-    moveit_config = MoveItConfigsBuilder("med14", package_name="med14_moveit_config").to_moveit_configs()
-    return generate_rsp_launch(moveit_config)
diff --git a/lbr_moveit_config/med14_moveit_config/launch/spawn_controllers.launch.py b/lbr_moveit_config/med14_moveit_config/launch/spawn_controllers.launch.py
deleted file mode 100644
index d972ac9e..00000000
--- a/lbr_moveit_config/med14_moveit_config/launch/spawn_controllers.launch.py
+++ /dev/null
@@ -1,7 +0,0 @@
-from moveit_configs_utils import MoveItConfigsBuilder
-from moveit_configs_utils.launches import generate_spawn_controllers_launch
-
-
-def generate_launch_description():
-    moveit_config = MoveItConfigsBuilder("med14", package_name="med14_moveit_config").to_moveit_configs()
-    return generate_spawn_controllers_launch(moveit_config)
diff --git a/lbr_moveit_config/med14_moveit_config/launch/static_virtual_joint_tfs.launch.py b/lbr_moveit_config/med14_moveit_config/launch/static_virtual_joint_tfs.launch.py
deleted file mode 100644
index 4f699127..00000000
--- a/lbr_moveit_config/med14_moveit_config/launch/static_virtual_joint_tfs.launch.py
+++ /dev/null
@@ -1,7 +0,0 @@
-from moveit_configs_utils import MoveItConfigsBuilder
-from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch
-
-
-def generate_launch_description():
-    moveit_config = MoveItConfigsBuilder("med14", package_name="med14_moveit_config").to_moveit_configs()
-    return generate_static_virtual_joint_tfs_launch(moveit_config)
diff --git a/lbr_moveit_config/med14_moveit_config/launch/warehouse_db.launch.py b/lbr_moveit_config/med14_moveit_config/launch/warehouse_db.launch.py
deleted file mode 100644
index 15be86b7..00000000
--- a/lbr_moveit_config/med14_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("med14", package_name="med14_moveit_config").to_moveit_configs()
-    return generate_warehouse_db_launch(moveit_config)
diff --git a/lbr_moveit_config/med14_moveit_config/package.xml b/lbr_moveit_config/med14_moveit_config/package.xml
index 1f93c814..79714686 100644
--- a/lbr_moveit_config/med14_moveit_config/package.xml
+++ b/lbr_moveit_config/med14_moveit_config/package.xml
@@ -30,19 +30,14 @@
        We don't include them by default to prevent installing gazebo and all its dependencies. -->
   <!-- <exec_depend>joint_trajectory_controller</exec_depend> -->
   <!-- <exec_depend>gazebo_ros_control</exec_depend> -->
-  <exec_depend>controller_manager</exec_depend>
   <exec_depend>lbr_description</exec_depend>
   <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>warehouse_ros_mongo</exec_depend>
   <exec_depend>xacro</exec_depend>