diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json
index 71051ee..ef74cef 100644
--- a/.devcontainer/devcontainer.json
+++ b/.devcontainer/devcontainer.json
@@ -7,7 +7,7 @@
   "workspaceFolder": "/workspace/src/lbrmed-ros",
   // Source the ROS workspace, the ${containerWorkspaceFolder} variable only gets replaced in this .json
   // Install dependencies for workspace except Gazebo packages as the OSRF version is installed
-  "postCreateCommand": "echo 'source /opt/ros/${ROS_DISTRO}/setup.bash' >> ${HOME}/.bashrc && echo 'source ${containerWorkspaceFolder}/devel/setup.bash' >> ${HOME}/.bashrc && /bin/sh .devcontainer/postCreate.sh",
+  "postCreateCommand": "echo 'source /opt/ros/${ROS_DISTRO}/setup.bash' >> ${HOME}/.bashrc && echo 'source /workspace/devel/setup.bash' >> ${HOME}/.bashrc && /bin/sh .devcontainer/postCreate.sh",
   "customizations": {
     "vscode": {
       "extensions": [
diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index d616374..e90ee68 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -23,4 +23,3 @@ jobs:
           ROS_DISTRO: noetic
           CATKIN_LINT: true
           CATKIN_LINT_ARGS: -W2
-          UPSTREAM_WORKSPACE: .rosinstall
diff --git a/.gitignore b/.gitignore
index a659ccf..27324f3 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,2 +1 @@
 .catkin_tools
-.vscode
diff --git a/.rosinstall b/.rosinstall
deleted file mode 100644
index 05a877c..0000000
--- a/.rosinstall
+++ /dev/null
@@ -1,8 +0,0 @@
-- git:
-    local-name: cartesian_controllers
-    uri: https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers.git
-    version: ros1
-- git:
-    local-name: ros_control_boilerplate
-    uri: https://github.com/Tuebel/ros_control_boilerplate.git
-    version: combined_robot_hw_draft
diff --git a/.vscode/copyright_template.txt b/.vscode/copyright_template.txt
new file mode 100644
index 0000000..7c8e624
--- /dev/null
+++ b/.vscode/copyright_template.txt
@@ -0,0 +1,3 @@
+@license BSD-3 https://opensource.org/licenses/BSD-3-Clause
+Copyright (c) $CURRENT_YEAR, Institute of Automatic Control - RWTH Aachen University
+All rights reserved. 
\ No newline at end of file
diff --git a/.vscode/tasks.json b/.vscode/tasks.json
new file mode 100644
index 0000000..066fa4b
--- /dev/null
+++ b/.vscode/tasks.json
@@ -0,0 +1,70 @@
+/* Copyright (c) 2020, Institute of Automatic Control - RWTH Aachen University
+   All rights reserved. */
+{
+  "version": "2.0.0",
+  "tasks": [
+    {
+      "label": "merge compile_commands.json",
+      "type": "shell",
+      "command": "cd /workspace && jq -s 'map(.[])' /workspace/build/**/compile_commands.json > /workspace/build/compile_commands.json",
+      "group": "none",
+      "problemMatcher": "$catkin-gcc"
+    },
+    {
+      "label": "catkin build workspace",
+      "type": "shell",
+      "command": "cd /workspace && catkin build && jq -s 'map(.[])' /workspace/build/**/compile_commands.json > /workspace/build/compile_commands.json",
+      "group": {
+        "kind": "build",
+        "isDefault": true
+      },
+      "problemMatcher": [
+        "$catkin-gcc"
+      ]
+    },
+    {
+      "label": "catkin build package",
+      "type": "shell",
+      "command": "cd ${fileDirname} && catkin build --this --workspace /workspace && jq -s 'map(.[])' /workspace/build/**/compile_commands.json > /workspace/build/compile_commands.json",
+      "problemMatcher": [
+        "$catkin-gcc"
+      ]
+    },
+    {
+      "label": "catkin test current",
+      "type": "shell",
+      "command": "cd ${fileDirname} && catkin build --this --workspace /workspace --cmake-args -DCMAKE_BUILD_TYPE=Debug && catkin test --no-deps --this --verbose",
+      "group": {
+        "kind": "test",
+        "isDefault": true
+      },
+      "problemMatcher": [
+        "$msCompile"
+      ]
+    },
+    {
+      "label": "catkin config debug",
+      "type": "shell",
+      "command": "cd /workspace && catkin config -j $(($(nproc)-1)) --cmake-args -DCMAKE_BUILD_TYPE=Debug -DCMAKE_EXPORT_COMPILE_COMMANDS=1",
+      "problemMatcher": [
+        "$msCompile"
+      ]
+    },
+    {
+      "label": "catkin config release",
+      "type": "shell",
+      "command": "cd /workspace && catkin config -j $(($(nproc)-1)) --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=1",
+      "problemMatcher": [
+        "$msCompile"
+      ]
+    },
+    {
+      "label": "catkin clean",
+      "type": "shell",
+      "command": "cd /workspace && catkin clean --yes",
+      "problemMatcher": [
+        "$msCompile"
+      ]
+    }
+  ]
+}
\ No newline at end of file
diff --git a/README.md b/README.md
index a285d98..c5d24a4 100644
--- a/README.md
+++ b/README.md
@@ -19,7 +19,7 @@ Please look at the READMEs in the specific hardware packages.
   * No need to patch a kernel as we do not use kernel interrupts only the standard `ros_control` loop.
   * You might observe connectivity errors if you launch additional loads, e.g. RViz, or use a network switch.
 * We provide a **development container**, so you can start without manually installing any dependencies.
-* For custom setups, **only ROS tools** like `wstool` / `vcs` and `rosdep` are required to install dependencies.
+* Otherwise, this package is simple to set up without manual steps using `wstool` / `vcs` and `rosdep`.
 * We built collision and visualization meshes for the LBR Med with internal electric connectors.
 
 # Install
@@ -53,12 +53,11 @@ For the Sunrise Project you have two ways to proceed:
 ### Installing Dependencies
 In the catkin workspace, run the following commands to install all workspace and system dependencies.
 ```bash
-wstool init src
-wstool merge -t src src/lbrmed-ros/.rosinstall
-wstool update -t src
 rosdep install --from-paths src --ignore-src -r -y
 ``` 
 
+If you want to use the [FZI Cartesian controllers](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers), you can find the installation steps in the `lbrmed_control` [README](./lbrmed_control/README.md).
+
 ### Building the workspace
 To build the packages in parallel, we use the [catkin_tools](https://catkin-tools.readthedocs.io/en/latest/installing.html). After installing them, run:
 ```bash
diff --git a/lbrmed_bringup/launch/hardware.launch b/lbrmed_bringup/launch/hardware.launch
index 0362467..f0405db 100644
--- a/lbrmed_bringup/launch/hardware.launch
+++ b/lbrmed_bringup/launch/hardware.launch
@@ -40,7 +40,7 @@
       <arg name="ft_sensor_ref_link" value="$(arg arm_id)_link8" />
       <arg name="control_loop_hz" value="$(arg control_loop_hz)" />
 
-      <arg name="load_controllers" value="cartesian_motion_control_handle PositionJointInterface_cartesian_motion_controller PositionJointInterface_abs_admittance_controller"/>
+      <arg name="load_controllers" value="PositionJointInterface_abs_admittance_controller"/>
       <arg name="controllers" value="joint_state_controller force_torque_sensor_controller $(arg hardware_interface)_trajectory_controller" />
     </include>
 
diff --git a/lbrmed_bringup/launch/simulation.launch b/lbrmed_bringup/launch/simulation.launch
index ec54e02..336e23c 100644
--- a/lbrmed_bringup/launch/simulation.launch
+++ b/lbrmed_bringup/launch/simulation.launch
@@ -23,7 +23,7 @@
     EffortJointInterface for grasping tasks
     PositionJointInterface for FZI cartesian controllers 
    -->
-  <arg name="hardware_interface" default="PositionJointInterface"  />
+  <arg name="hardware_interface" default="PositionJointInterface" />
   <arg name="enable_safety_limits" default="true" />
   <!-- Frequency of the control loop, Position requires ≤ 10ms, Effort ≤ 5ms response -->
   <arg name="control_loop_hz" default="200" />
@@ -52,7 +52,7 @@
   </include>
 
   <!-- ROS control boilerplate SimHWInterface -->
-  <include file="$(find lbrmed_control)/launch/lbrmed_sim_hw.launch" >
+  <include file="$(find lbrmed_control)/launch/lbrmed_sim_hw.launch">
     <arg name="arm_id" value="$(arg arm_id)" />
     <arg name="control_loop_hz" value="$(arg control_loop_hz)" />
   </include>
@@ -65,7 +65,7 @@
     <arg name="ft_sensor_ref_link" value="$(arg arm_id)_link8" />
     <arg name="control_loop_hz" value="$(arg control_loop_hz)" />
     <arg name="controllers" value="joint_state_controller $(arg hardware_interface)_trajectory_controller" />
-    <arg name="load_controllers" value="cartesian_motion_control_handle  PositionJointInterface_cartesian_motion_controller PositionJointInterface_abs_admittance_controller" />
+    <arg name="load_controllers" value="PositionJointInterface_abs_admittance_controller" />
   </include>
 
   <node name="rqt_controller_manager" pkg="rqt_controller_manager" type="rqt_controller_manager" />
diff --git a/lbrmed_bringup/package.xml b/lbrmed_bringup/package.xml
index 5951a84..9a5291c 100644
--- a/lbrmed_bringup/package.xml
+++ b/lbrmed_bringup/package.xml
@@ -8,7 +8,6 @@
   <license>BSD-3</license>
 
   <buildtool_depend>catkin</buildtool_depend>
-  <exec_depend>cartesian_controllers</exec_depend>
   <exec_depend>joint_state_publisher</exec_depend>
   <exec_depend>lbrmed_control</exec_depend>
   <exec_depend>lbrmed_description</exec_depend>
diff --git a/lbrmed_control/.rosinstall b/lbrmed_control/.rosinstall
new file mode 100644
index 0000000..7fdd3b6
--- /dev/null
+++ b/lbrmed_control/.rosinstall
@@ -0,0 +1,4 @@
+- git:
+    local-name: cartesian_controllers
+    uri: https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers.git
+    version: ros1
\ No newline at end of file
diff --git a/lbrmed_control/CMakeLists.txt b/lbrmed_control/CMakeLists.txt
index 2abf30b..f890eed 100644
--- a/lbrmed_control/CMakeLists.txt
+++ b/lbrmed_control/CMakeLists.txt
@@ -11,9 +11,9 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp)
 
 catkin_package(
- INCLUDE_DIRS include  
- LIBRARIES ${PROJECT_NAME}  
- CATKIN_DEPENDS
+  INCLUDE_DIRS include  
+  LIBRARIES ${PROJECT_NAME}  
+  CATKIN_DEPENDS
   actionlib
   controller_interface
   lbrmed_msgs
diff --git a/lbrmed_control/README.md b/lbrmed_control/README.md
index ee1019f..0683587 100644
--- a/lbrmed_control/README.md
+++ b/lbrmed_control/README.md
@@ -18,10 +18,21 @@ Provides launch files to load and start a set of default controllers.
 - `joint_state_controller`: Implements a controller which re-publishes the joint states of the robot.
 - `force_torque_sensor_controller`: Implements a controller which re-publishes the force and torque sensors of the robot.
 
+## FZI Cartesian controllers
+The config contains entries for the [FZI Cartesian controllers](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers).
+However, this package is intended to be a self-contained build and source dependencies are reduced to a minimum.
+To install the Cartesian controllers, run the following commands in the workspace root:
+```shell
+wstool init src
+wstool merge -t src src/lbrmed-ros/lbrmed_control/.rosinstall
+wstool update -t src
+```
+Then re-build and -source the whole workspace
+
 ## Custom controllers
 - `PositionJointInterface_abs_admittance_controller`: Implements an absolute force-based admittance controller.
 Compared to a normal admittance controller, the robot approaches an object until the total force exceeds a limit and stops.
-I tried implementing a similar behavior using the [FZI cartesian controllers](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers), but it deviated from the commanded position, no matter how I tuned the parameters.
+I tried implementing a similar behavior using the [FZI Cartesian controllers](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers), but it deviated from the commanded position, no matter how I tuned the parameters.
 This controller uses good old KDL inverse kinematics and monitors the forces in the ROS control loop.
 Compared to the FZI version, this implementation is **less robust near singularities**.
   - Offers `abs_admittance_control` action:
diff --git a/lbrmed_control/launch/lbrmed_control.launch b/lbrmed_control/launch/lbrmed_control.launch
index 961d7f1..ff1b7bb 100644
--- a/lbrmed_control/launch/lbrmed_control.launch
+++ b/lbrmed_control/launch/lbrmed_control.launch
@@ -16,7 +16,7 @@
      -->
     <arg name="controllers" default="joint_state_controller force_torque_sensor_controller PositionJointInterface_trajectory_controller" />
     <!-- These controllers are only loaded and not started -->
-    <arg name="load_controllers" default=" cartesian_motion_control_handle PositionJointInterface_cartesian_motion_controller PositionJointInterface_abs_admittance_controller" />
+    <arg name="load_controllers" default="PositionJointInterface_abs_admittance_controller" />
 
     <!-- Frequency of the control loop, Position requires ≤ 10ms, Effort ≤ 5ms response -->
     <arg name="control_loop_hz" default="200" />
diff --git a/lbrmed_control/package.xml b/lbrmed_control/package.xml
index 3169cd8..764d5e2 100644
--- a/lbrmed_control/package.xml
+++ b/lbrmed_control/package.xml
@@ -8,8 +8,6 @@
   <license>BSD-3</license>
 
   <buildtool_depend>catkin</buildtool_depend>
-  <exec_depend>cartesian_compliance_controller</exec_depend>
-  <exec_depend>cartesian_motion_controller</exec_depend>
   <exec_depend>controller_manager</exec_depend>
   <exec_depend>effort_controllers</exec_depend>
   <exec_depend>joint_state_controller</exec_depend>
diff --git a/lbrmed_hw_fri/CMakeLists.txt b/lbrmed_hw_fri/CMakeLists.txt
index cc2169c..141cbb4 100644
--- a/lbrmed_hw_fri/CMakeLists.txt
+++ b/lbrmed_hw_fri/CMakeLists.txt
@@ -62,10 +62,6 @@ install(TARGETS ${PROJECT_NAME}_node
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
-# Install Plugin
-install(FILES ${PROJECT_NAME}_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
-
-
 ############
 # Unit Tests
 ############
diff --git a/lbrmed_hw_fri/lbrmed_hw_fri_plugin.xml b/lbrmed_hw_fri/lbrmed_hw_fri_plugin.xml
deleted file mode 100644
index f4bd193..0000000
--- a/lbrmed_hw_fri/lbrmed_hw_fri_plugin.xml
+++ /dev/null
@@ -1,7 +0,0 @@
-<library path="lib/liblbrmed_hw_fri">
-  <class name="lbrmed_hw_fri/HardwareInterface" type="lbrmed_hw_fri::HardwareInterface" base_class_type="hardware_interface::RobotHW">
-    <description>
-            Interface for an KUKA LBR lbrmed arm.
-    </description>
-  </class>
-</library>
diff --git a/lbrmed_hw_fri/package.xml b/lbrmed_hw_fri/package.xml
index 48132f9..55033fa 100644
--- a/lbrmed_hw_fri/package.xml
+++ b/lbrmed_hw_fri/package.xml
@@ -20,8 +20,4 @@
   <exec_depend>hardware_interface</exec_depend>
   <exec_depend>lbrmed_control</exec_depend>
 
-  <export>
-    <hardware_interface plugin="${prefix}/lbrmed_hw_fri_plugin.xml" />
-  </export>
-
 </package>
diff --git a/lbrmed_hw_fri/src/lbrmed_hw_fri.cpp b/lbrmed_hw_fri/src/lbrmed_hw_fri.cpp
index 92ec4b6..fbe7b32 100644
--- a/lbrmed_hw_fri/src/lbrmed_hw_fri.cpp
+++ b/lbrmed_hw_fri/src/lbrmed_hw_fri.cpp
@@ -18,7 +18,6 @@
 #include <kdl_parser/kdl_parser.hpp>
 // ROS
 #include <ros/console.h>
-#include <ros_control_boilerplate/combinable_generic_hw.h>
 #include <rosparam_shortcuts/rosparam_shortcuts.h>
 #include <transmission_interface/transmission_interface.h>
 // KUKA lib
@@ -212,6 +211,4 @@ double HardwareInterface::low_pass_filter(double y_k, double u_k, double T, doub
   }
 }
 
-}  // namespace lbrmed_hw_fri
-PLUGINLIB_EXPORT_CLASS(ros_control_boilerplate::CombinableGenericHW<lbrmed_hw_fri::HardwareInterface>,
-                       hardware_interface::RobotHW)
\ No newline at end of file
+}  // namespace lbrmed_hw_fri
\ No newline at end of file
diff --git a/lbrmed_moveit_config/launch/moveit.rviz b/lbrmed_moveit_config/launch/moveit.rviz
index cee13e5..d70da18 100644
--- a/lbrmed_moveit_config/launch/moveit.rviz
+++ b/lbrmed_moveit_config/launch/moveit.rviz
@@ -5,9 +5,10 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Global Options1
+        - /MotionPlanning1
         - /MotionPlanning1/Planned Path1
       Splitter Ratio: 0.5415094494819641
-    Tree Height: 390
+    Tree Height: 285
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -120,7 +121,7 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-        Loop Animation: false
+        Loop Animation: true
         Robot Alpha: 0.5
         Robot Color: 150; 50; 150
         Show Robot Collision: false
@@ -351,7 +352,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/XYOrbit
-      Distance: 3.281726837158203
+      Distance: 2.0772390365600586
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -359,17 +360,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: 0
-        Y: 0
-        Z: 0
+        X: 0.13066630065441132
+        Y: -0.12184453010559082
+        Z: -5.960464477539063e-08
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.260398268699646
+      Pitch: 0.3603981137275696
       Target Frame: lbrmed14_link0
-      Yaw: 0.5303983688354492
+      Yaw: 1.4153984785079956
     Saved: ~
 Window Geometry:
   Displays:
@@ -383,9 +384,9 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd00000003000000000000023100000391fc020000000cfb000000100044006900730070006c006100790073010000003b00000217000000c700fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000258000001740000016900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001f1000002070000000000000000fb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007202000000000000000000000280000001e0fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007202000000000000000000000280000001e0fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006702000000000000001b000002b60000020dfb000000280020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007202000000000000000000000280000001e0fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007202000000000000000000000280000001e0000000010000012a00000391fc0200000001fb0000000a00560069006500770073010000003b00000391000000a000ffffff000000030000059e00000174fc0100000001fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000021a0000059e00000000000000000000040d0000039100000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000300000000000001ca00000391fc020000000cfb000000100044006900730070006c006100790073010000003b000001ae000000c700fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001ef000001dd0000016900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001f1000002070000000000000000fb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007202000000000000000000000280000001e0fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007202000000000000000000000280000001e0fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006702000000000000001b000002b60000020dfb000000280020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007202000000000000000000000280000001e0fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007202000000000000000000000280000001e0000000010000012a00000391fc0200000001fb0000000a00560069006500770073010000003b00000391000000a000ffffff000000030000059e00000174fc0100000001fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000021a0000059e0000000000000000000004740000039100000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
   Width: 1908
-  X: -34
-  Y: 17
+  X: 0
+  Y: 4