diff --git a/mur/mur_control/launch/global_tcp_pose_publisher.launch b/mur/mur_control/launch/global_tcp_pose_publisher.launch
new file mode 100644
index 00000000..cffa054b
--- /dev/null
+++ b/mur/mur_control/launch/global_tcp_pose_publisher.launch
@@ -0,0 +1,22 @@
+<launch>
+    <arg name="world_T_base_topic" default="/mur620/mir_pose_stamped_simple"/>
+    <arg name="base_T_ee_topic" default="/mur620/UR10_l/tcp_pose"/> <!-- TODO: could be done with static transform in future -->
+    <arg name="world_T_ee_topic" default="/global_nozzle_pose"/>
+    <arg name="static_tf_included" default="true"/>
+    <arg name="static_tf_parent" default="mur620/base_footprint"/>
+    <arg name="static_tf_child" default="mur620/UR10_l/base_link"/>
+    <arg name="world_frame" default="map"/> <!-- just for publisher frame -->
+    
+    <node name="pose_composer" pkg="mur_control" type="pose_composer.py" output="screen">
+        <!-- Topic Remappings -->
+        <remap from="world_T_base" to="$(arg world_T_base_topic)"/>
+        <remap from="base_T_ee" to="$(arg base_T_ee_topic)"/>
+        <remap from="world_T_ee" to="$(arg world_T_ee_topic)"/><!-- publisher -->
+
+        <param name="world_frame" value="$(arg world_frame)"/>
+
+        <param name="static_tf_included" value="$(arg static_tf_included)"/>
+        <param name="static_tf_parent" value="$(arg static_tf_parent)"/>
+        <param name="static_tf_child" value="$(arg static_tf_child)"/>
+    </node>
+</launch>
\ No newline at end of file
diff --git a/mur/mur_control/launch/multi_twist_controller_sim.launch b/mur/mur_control/launch/multi_twist_controller_sim.launch
new file mode 100644
index 00000000..f12458cb
--- /dev/null
+++ b/mur/mur_control/launch/multi_twist_controller_sim.launch
@@ -0,0 +1,36 @@
+<launch>
+
+    
+    <arg name="robot1_name"  default="mur620a"/>
+    <arg name="robot2_name"  default="mur620b"/>
+
+
+    <include file="$(find mur_control)/launch/twist_controller_sim.launch">
+        <arg name="robot_name"  value="$(arg robot1_name)"/>
+        <arg name="group_vel_controller_topic" value="/$(arg robot1_name)/joint_group_vel_controller_l/unsafe/command"/>
+        <arg name="commanded_twist_topic" value="/$(arg robot1_name)/UR10_l/commanded_twist"/>
+        <arg name="move_group_name" value="UR_arm_l"/>
+    </include>
+
+    <include file="$(find mur_control)/launch/twist_controller_sim.launch">
+        <arg name="robot_name"  value="$(arg robot1_name)"/>
+        <arg name="group_vel_controller_topic" value="/$(arg robot1_name)/joint_group_vel_controller_r/unsafe/command"/>
+        <arg name="commanded_twist_topic" value="/$(arg robot1_name)/UR10_r/commanded_twist"/>
+        <arg name="move_group_name" value="UR_arm_r"/>
+    </include>
+
+    <include file="$(find mur_control)/launch/twist_controller_sim.launch">
+        <arg name="robot_name"  value="$(arg robot2_name)"/>
+        <arg name="group_vel_controller_topic" value="/$(arg robot2_name)/joint_group_vel_controller_l/unsafe/command"/>
+        <arg name="commanded_twist_topic" value="/$(arg robot2_name)/UR10_l/commanded_twist"/>
+        <arg name="move_group_name" value="UR_arm_l"/>
+    </include>
+
+    <include file="$(find mur_control)/launch/twist_controller_sim.launch">
+        <arg name="robot_name"  value="$(arg robot2_name)"/>
+        <arg name="group_vel_controller_topic" value="/$(arg robot2_name)/joint_group_vel_controller_r/unsafe/command"/>
+        <arg name="commanded_twist_topic" value="/$(arg robot2_name)/UR10_r/commanded_twist"/>
+        <arg name="move_group_name" value="UR_arm_r"/>
+    </include>
+
+</launch>
\ No newline at end of file
diff --git a/mur/mur_control/launch/twist_controller_sim.launch b/mur/mur_control/launch/twist_controller_sim.launch
new file mode 100644
index 00000000..ec72ecff
--- /dev/null
+++ b/mur/mur_control/launch/twist_controller_sim.launch
@@ -0,0 +1,18 @@
+<launch>
+
+    <arg name="robot_name"  default="mur620a"/>
+    <arg name="group_vel_controller_topic" default="/$(arg robot_name)/joint_group_vel_controller_r/unsafe/command"/>
+    <arg name="commanded_twist_topic" default="/$(arg robot_name)/UR10_r/commanded_twist"/>
+    <arg name="move_group_name" default="UR_arm_r"/>
+
+    <group ns="$(arg robot_name)">
+
+        <node name="twist_controller_sim_$(arg robot_name)_$(arg move_group_name)" pkg="mur_control" type="twist_controller_sim.py" output="screen">
+            <param name="robot_name" value="$(arg robot_name)" />
+            <param name="group_vel_controller_topic" value="$(arg group_vel_controller_topic)" />
+            <param name="commanded_twist_topic" value="$(arg commanded_twist_topic)" />
+            <param name="move_group_name" value="$(arg move_group_name)" />  
+        </node>
+    </group>
+
+</launch>
\ No newline at end of file
diff --git a/mur/mur_control/scripts/__pycache__/ur_dual_arm_collision_dist.cpython-38.pyc b/mur/mur_control/scripts/__pycache__/ur_dual_arm_collision_dist.cpython-38.pyc
deleted file mode 100644
index cbccd333..00000000
Binary files a/mur/mur_control/scripts/__pycache__/ur_dual_arm_collision_dist.cpython-38.pyc and /dev/null differ
diff --git a/mur/mur_control/scripts/global_tcp_pose_publisher.py b/mur/mur_control/scripts/global_tcp_pose_publisher.py
index 4e4b27ae..2db11eca 100755
--- a/mur/mur_control/scripts/global_tcp_pose_publisher.py
+++ b/mur/mur_control/scripts/global_tcp_pose_publisher.py
@@ -13,7 +13,7 @@ def __init__(self):
         self.UR_base_link_name = rospy.get_param('~UR_base_link_name', 'mur620a/UR10_l/base_link_inertia')
         self.base_frame = rospy.get_param('~base_frame', 'map')
         self.local_TCP_pose_topic = rospy.get_param('~local_TCP_pose_topic', '/mur620a/UR10_l/ur_calibrated_pose')
-        self.static_mode = rospy.get_param('~static_mode', False)
+        self.static_mode = rospy.get_param('~static_mode', True)
 
         self.local_TCP_pose = Pose()
 
diff --git a/mur/mur_control/scripts/pose_composer.py b/mur/mur_control/scripts/pose_composer.py
new file mode 100755
index 00000000..a032e6bf
--- /dev/null
+++ b/mur/mur_control/scripts/pose_composer.py
@@ -0,0 +1,84 @@
+#!/usr/bin/env python3
+import rospy
+
+from geometry_msgs.msg import Pose, PoseStamped
+from tf import TransformListener
+from tf.transformations import quaternion_matrix, quaternion_from_matrix, concatenate_matrices
+
+import numpy as np
+
+# Listens to Pose messages and calculates the kinematic chain to publish the total Pose
+# in: world_T_base, base_T_ee; out: world_T_ee
+def pose_to_matrix(pose:Pose):
+    """Convert Pose to transformation matrix"""
+    trans = [pose.position.x, pose.position.y, pose.position.z]
+    rot = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
+    matrix = quaternion_matrix(rot)
+    matrix[0:3, 3] = trans
+    return matrix
+
+def matrix_to_pose(matrix: np.ndarray) -> Pose:
+    """Convert transformation matrix to Pose"""
+    pose = Pose()
+    translation = matrix[0:3, 3]
+    rotation = quaternion_from_matrix(matrix)
+    pose.position.x, pose.position.y, pose.position.z = translation
+    pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w = rotation
+    return pose
+class TotalPose:
+    def __init__(self):
+        # msg_class_base,_,_=rostopic.get_topic_class("world_T_base", blocking=True)
+        self.world_T_base = np.eye(4)
+        self.base_T_ee = np.eye(4)
+        self.world_T_ee = PoseStamped()
+        self.world_T_ee.header.frame_id = rospy.get_param("~world_frame", "map")
+
+        # In case there is a static transform between the 2 topics:
+        self.static_tf_included = rospy.get_param("~static_tf_included", False)
+        static_tf_parent = rospy.get_param("~static_tf_parent", "mur620/base_footprint")
+        static_tf_child = rospy.get_param("~static_tf_child", "mur620/UR10_l/base_link")
+        self.static_T = None
+        if self.static_tf_included:
+            tf_listener = TransformListener()
+
+            while not rospy.is_shutdown():
+                try:
+                    tf_listener.waitForTransform(static_tf_parent, static_tf_child, rospy.Time(0), rospy.Duration(5.0))
+                    static_trans, static_rot = tf_listener.lookupTransform(static_tf_parent, static_tf_child, rospy.Time(0))
+                    break
+                except Exception as e:
+                    rospy.logwarn(f"Could not get static transform: {e}. Retrying...")
+                    rospy.sleep(1.0)
+            self.static_T = quaternion_matrix([static_rot[0], static_rot[1], static_rot[2], static_rot[3]])
+            self.static_T[0:3, 3] = static_trans
+
+        self.world_T_base_sub = rospy.Subscriber("world_T_base", PoseStamped, self.world_T_base_callback)
+        self.base_T_ee_sub = rospy.Subscriber("base_T_ee", PoseStamped, self.base_T_ee_callback)
+        self.world_T_ee_pub = rospy.Publisher("world_T_ee", PoseStamped, queue_size=1)
+
+    def world_T_base_callback(self, msg: PoseStamped):
+        self.world_T_base = pose_to_matrix(msg.pose)
+        self.world_T_ee.header.stamp = msg.header.stamp
+        self.update_world_T_ee()
+
+    def base_T_ee_callback(self, msg: PoseStamped):
+        base_T_ee = pose_to_matrix(msg.pose)
+        if self.static_tf_included:
+            base_T_ee = concatenate_matrices(self.static_T, base_T_ee)
+        self.base_T_ee = base_T_ee
+
+        self.world_T_ee.header.stamp = msg.header.stamp
+        self.update_world_T_ee()
+
+    def update_world_T_ee(self):
+        
+        # self.world_T_ee.pose = transform_pose_by_pose(self.world_T_base, self.base_T_ee, (False, False))
+
+        world_T_ee = concatenate_matrices(self.world_T_base, self.base_T_ee)    
+        self.world_T_ee.pose = matrix_to_pose(world_T_ee)
+        self.world_T_ee_pub.publish(self.world_T_ee)
+
+if __name__ == "__main__":
+    rospy.init_node("total_pose")
+    total_pose = TotalPose()
+    rospy.spin()
\ No newline at end of file
diff --git a/mur/mur_description/urdf/mur_620.gazebo.xacro b/mur/mur_description/urdf/mur_620.gazebo.xacro
index ac4e0b34..7370169a 100644
--- a/mur/mur_description/urdf/mur_620.gazebo.xacro
+++ b/mur/mur_description/urdf/mur_620.gazebo.xacro
@@ -34,6 +34,7 @@
 	<xacro:if value="${sim}">
 		<xacro:include filename="$(find schunk_emh_rp_45)/urdf/schunk_emh_rp_045.urdf.xacro"/>
 		<xacro:include filename="$(find keyence_lj_x)/urdf/keyence_lj_x.urdf.xacro"/>
+		<xacro:include filename="$(find bauschaum_ee)/urdf/bauschaum_ee.urdf.xacro"/>
 	</xacro:if>
 	<!-- Init the MiR200 marco that is also used for the hardware -->
 	<xacro:mir_600 />
@@ -106,6 +107,10 @@
 			<xacro:keyence_lj_x tf_prefix="UR10_r"	tool_link="UR10_r/flange"/>
 			<xacro:keyence_lj_x tf_prefix="UR10_l"	tool_link="UR10_l/flange"/>
 		</xacro:if>
+		<xacro:if value="${tool == 'bauschaum_ee'}">
+			<xacro:bauschaum_ee tf_prefix="UR10_r"	tool_link="UR10_r/flange"/>
+			<xacro:bauschaum_ee  tf_prefix="UR10_l"	tool_link="UR10_l/flange"/>
+		</xacro:if>
 	</xacro:if>
 
     <!-- <xacro:include filename="$(find print_sim)/urdf/keyence_line_laser.gazebo.xacro" /> 
diff --git a/mur/mur_examples/scripts/set_virtual_object_pose.py b/mur/mur_examples/scripts/set_virtual_object_pose.py
index 95d8822c..ee49dbd7 100755
--- a/mur/mur_examples/scripts/set_virtual_object_pose.py
+++ b/mur/mur_examples/scripts/set_virtual_object_pose.py
@@ -5,22 +5,25 @@
 def set_target_pose():
     # Initialize the ROS node
     rospy.init_node('set_virtual_object_pose')
-
+    pose = rospy.get_param('~pose', [0.0, 0.0, 1.5, 0, 0, 0.3826834, 0.9238795])
+    pose_topic = rospy.get_param('~pose_topic', '/virtual_object/set_pose')
     # Create a publisher for the target pose
-    pub = rospy.Publisher('/virtual_object/set_pose', PoseStamped, queue_size=10)
+    pub = rospy.Publisher(pose_topic, PoseStamped, queue_size=10)
+
+    rospy.loginfo('Setting the target pose to: {}'.format(pose))
 
     rospy.sleep(1)
 
     # Create a PoseStamped message
     target_pose = PoseStamped()
     target_pose.header.frame_id = 'map'  # Set the frame ID
-    target_pose.pose.position.x = 0.0  # Set the position (x-coordinate)
-    target_pose.pose.position.y = 0.0  # Set the position (y-coordinate)
-    target_pose.pose.position.z = 1.5  # Set the position (z-coordinate)
-    target_pose.pose.orientation.x = 0.0  # Set the orientation (x-coordinate)
-    target_pose.pose.orientation.y = 0.0  # Set the orientation (y-coordinate)
-    target_pose.pose.orientation.z = 0.0  # Set the orientation (z-coordinate)
-    target_pose.pose.orientation.w = 1.0  # Set the orientation (w-coordinate)
+    target_pose.pose.position.x = pose[0]  # Set the position (x-coordinate)
+    target_pose.pose.position.y = pose[1]  # Set the position (y-coordinate)
+    target_pose.pose.position.z = pose[2]  # Set the position (z-coordinate)
+    target_pose.pose.orientation.x = pose[3]  # Set the orientation (x-coordinate)
+    target_pose.pose.orientation.y = pose[4]  # Set the orientation (y-coordinate)
+    target_pose.pose.orientation.z = pose[5]  # Set the orientation (z-coordinate)
+    target_pose.pose.orientation.w = pose[6]  # Set the orientation (w-coordinate)
 
     # Publish the target pose
     pub.publish(target_pose)
diff --git a/mur/mur_launch_hardware/launch/general_mur600.launch b/mur/mur_launch_hardware/launch/general_mur600.launch
index 67edf3e3..8af82710 100644
--- a/mur/mur_launch_hardware/launch/general_mur600.launch
+++ b/mur/mur_launch_hardware/launch/general_mur600.launch
@@ -21,9 +21,9 @@
     <arg name="ur_l_rpy" default="0.0 0.0 0.0"/>
     <arg name="ur_r_xyz" default="0.0 0.0 0.0"/>
     <arg name="ur_r_rpy" default="0.0 0.0 3.141596"/>
-    <arg name="cmplt_robot_descr" default="$(find mur_description)/urdf/mur_620.gazebo.xacro simulate_camera:= $(arg simulate_camera) use_lift:=$(arg use_lift) sim:=false tool:=$(arg tool) ur_l_rpy:='$(arg ur_l_rpy)' ur_r_rpy:='$(arg ur_r_rpy)' ur_l_xyz:='$(arg ur_l_xyz)' ur_r_xyz:='$(arg ur_r_xyz)'" /> <!-- Complete robot description -->
+    <arg name="cmplt_robot_descr" default="$(find mur_description)/urdf/mur_620.gazebo.xacro use_lift:=$(arg use_lift) tool:=$(arg tool) simulate_camera:=$(arg simulate_camera) sim:=false ur_l_rpy:='$(arg ur_l_rpy)' ur_r_rpy:='$(arg ur_r_rpy)' ur_l_xyz:='$(arg ur_l_xyz)' ur_r_xyz:='$(arg ur_r_xyz)'" /> <!-- Complete robot description -->
     <arg name="localization_type" default="mocap" /> <!-- mocap or amcl or robot_pose or odom-->
-   
+
     <!-- Bringup the base of the mur -->
 
     <!-- TODO: disable robot_state_publisher of mir if joint states are used by other publisher. else: remap robot_description? -->
@@ -112,6 +112,14 @@
                 <param name="UR_base_link_name" value="$(arg tf_prefix)/$(arg left_arm_group_name)/base_link_inertia" />
                 <param name="local_TCP_pose_topic" value="ur_calibrated_pose" />
             </node>
+            <!-- TODO: change, but has to be tested first! -->
+            <!-- <include file="$(find mur_control)/launch/global_tcp_pose_publisher.launch">
+                <arg name="world_T_base_topic" default="/$(arg tf_prefix)/mir_pose_stamped_simple"/>
+                <arg name="base_T_ee_topic" value="ur_calibrated_pose"/>
+                <arg name="world_T_ee_topic" value="global_tcp_pose"/>
+                <arg name="static_tf_parent" value="$(arg tf_prefix)/base_footprint"/>
+                <arg name="static_tf_child" value="$(arg tf_prefix)/$(arg left_arm_group_name)/base_link"/>
+            </include> -->
 
 
         </group>
diff --git a/mur/mur_launch_hardware/launch/mur620a.launch b/mur/mur_launch_hardware/launch/mur620a.launch
index fcffa088..097cad3e 100644
--- a/mur/mur_launch_hardware/launch/mur620a.launch
+++ b/mur/mur_launch_hardware/launch/mur620a.launch
@@ -39,5 +39,9 @@
         <arg name="ur_r_rpy" value="$(arg ur_r_rpy)"/>
     </include>
 
+    <!-- static transform_publisher node to complete the tf-tree -->
+    <node pkg="tf" type="static_transform_publisher" name="dummy_lift_$(arg tf_prefix)_$(arg left_arm_group_name)" args="0 0 0 0 0 0 $(arg tf_prefix)/left_lift_bottom $(arg tf_prefix)/left_lift_top 100" />
+    <node pkg="tf" type="static_transform_publisher" name="dummy_lift_$(arg tf_prefix)_$(arg right_arm_group_name)" args="0 0 0 0 0 0 $(arg tf_prefix)/right_lift_bottom $(arg tf_prefix)/right_lift_top 100" />
+
 
 </launch>
\ No newline at end of file
diff --git a/mur/mur_launch_hardware/launch/mur620b.launch b/mur/mur_launch_hardware/launch/mur620b.launch
index a857ae5d..3af8290b 100644
--- a/mur/mur_launch_hardware/launch/mur620b.launch
+++ b/mur/mur_launch_hardware/launch/mur620b.launch
@@ -34,7 +34,14 @@
         <arg name="kinematic_config_l" value="$(find ur_launch_hardware)/calibration/calibration_UR10_19.yaml"/>
         <arg name="kinematic_config_r" value="$(find ur_launch_hardware)/calibration/calibration_UR10_17.yaml"/>
         <arg name="localization_type" value="$(arg localization_type)"/>
+        <arg name="ur_l_xyz" value="$(arg ur_l_xyz)"/>
+        <arg name="ur_l_rpy" value="$(arg ur_l_rpy)"/>
+        <arg name="ur_r_xyz" value="$(arg ur_r_xyz)"/>
+        <arg name="ur_r_rpy" value="$(arg ur_r_rpy)"/>
     </include>
 
+    <!-- static transform_publisher node to complete the tf-tree -->
+    <node pkg="tf" type="static_transform_publisher" name="dummy_lift_$(arg tf_prefix)_$(arg left_arm_group_name)" args="0 0 0 0 0 0 $(arg tf_prefix)/left_lift_bottom $(arg tf_prefix)/left_lift_top 100" />
+    <node pkg="tf" type="static_transform_publisher" name="dummy_lift_$(arg tf_prefix)_$(arg right_arm_group_name)" args="0 0 0 0 0 0 $(arg tf_prefix)/right_lift_bottom $(arg tf_prefix)/right_lift_top 100" />
 
 </launch>
\ No newline at end of file
diff --git a/mur/mur_launch_sim/launch/mur_620.launch b/mur/mur_launch_sim/launch/mur_620.launch
index 446f1147..52108b38 100644
--- a/mur/mur_launch_sim/launch/mur_620.launch
+++ b/mur/mur_launch_sim/launch/mur_620.launch
@@ -96,10 +96,14 @@
 				<param name="ur_prefix" value="$(arg prefix)_l/" />
 				<remap from="joint_states" to="/$(arg mur_ns)/joint_states" />
 			</node>
-			<node name="global_tcp_pose_publisher" pkg="mur_control" type="global_tcp_pose_publisher.py" output="screen">
-				<param name="UR_base_link_name" value="$(arg mur_ns)/$(arg prefix)_l/base_link" />
-				<param name="local_TCP_pose_topic" value="/$(arg mur_ns)/$(arg prefix)_l/tcp_pose" />
-			</node>
+
+			<include file="$(find mur_control)/launch/global_tcp_pose_publisher.launch">
+				<arg name="world_T_base_topic" default="/$(arg tf_prefix)/mir_pose_stamped_simple"/>
+                <arg name="base_T_ee_topic" value="tcp_pose"/>
+                <arg name="world_T_ee_topic" value="global_tcp_pose"/>
+                <arg name="static_tf_parent" value="$(arg tf_prefix)/base_footprint"/>
+                <arg name="static_tf_child" value="$(arg tf_prefix)/$(arg prefix)_l/base_link"/>
+            </include>
 		</group>
 
 		
@@ -114,14 +118,17 @@
 				<param name="ur_prefix" value="$(arg prefix)_r/" />
 				<remap from="joint_states" to="/$(arg mur_ns)/joint_states" />
 			</node>
-			<node name="global_tcp_pose_publisher" pkg="mur_control" type="global_tcp_pose_publisher.py" output="screen">
-				<param name="UR_base_link_name" value="$(arg mur_ns)/$(arg prefix)_r/base_link" />
-				<param name="local_TCP_pose_topic" value="/$(arg mur_ns)/$(arg prefix)_r/tcp_pose" />
-			</node>
+			<include file="$(find mur_control)/launch/global_tcp_pose_publisher.launch">
+				<arg name="world_T_base_topic" default="/$(arg tf_prefix)/mir_pose_stamped_simple"/>
+                <arg name="base_T_ee_topic" value="tcp_pose"/>
+                <arg name="world_T_ee_topic" value="global_tcp_pose"/>
+                <arg name="static_tf_parent" value="$(arg tf_prefix)/base_footprint"/>
+                <arg name="static_tf_child" value="$(arg tf_prefix)/$(arg prefix)_r/base_link"/>
+            </include>
 		</group>
 
 		<!-- use feedback instead (ur_controllers_match twist_controller_fb )-->
-		<group if = "$(arg use_inverse_differential_kinematics)">
+		<!-- <group if = "$(arg use_inverse_differential_kinematics)">
 			<node name="twist_controller_sim_r" pkg="mur_control" type="twist_controller_sim.py" output="screen" launch-prefix="bash -c 'sleep $(arg node_start_delay); $0 $@' " >
 				<param name="robot_name" value="$(arg mur_ns)" />
 				<param name="group_vel_controller_topic" value="joint_group_vel_controller_r/unsafe/command" />
@@ -135,7 +142,7 @@
 				<param name="commanded_twist_topic" value="$(arg prefix)_l/commanded_twist" />
 				<param name="move_group_name" value="UR_arm_l" />
 			</node>
-		</group>
+		</group> -->
 
 
 	
diff --git a/mur/mur_moveit_config/config/mur620.srdf.xacro b/mur/mur_moveit_config/config/mur620.srdf.xacro
index a815b0bb..f7d8c27a 100644
--- a/mur/mur_moveit_config/config/mur620.srdf.xacro
+++ b/mur/mur_moveit_config/config/mur620.srdf.xacro
@@ -198,6 +198,14 @@
         <joint name="$(arg prefix)_r/wrist_2_joint" value="1.57079632679"/>
         <joint name="$(arg prefix)_r/wrist_3_joint" value="0"/>
     </group_state>
+    <group_state name="handling_position_wide_lift_mur620c" group="UR_arm_r">
+        <joint name="$(arg prefix)_r/elbow_joint" value="-1.57079632679"/>
+        <joint name="$(arg prefix)_r/shoulder_lift_joint" value="-1.57079632679"/>
+        <joint name="$(arg prefix)_r/shoulder_pan_joint" value="-0.23640963340626803"/>
+        <joint name="$(arg prefix)_r/wrist_1_joint" value="-1.57079632679"/>
+        <joint name="$(arg prefix)_r/wrist_2_joint" value="1.57079632679"/>
+        <joint name="$(arg prefix)_r/wrist_3_joint" value="0"/>
+    </group_state>
 
 	<group_state name="home" group="UR_arm_both">
         <joint name="$(arg prefix)_l/elbow_joint" value="1.6141"/>
@@ -298,14 +306,6 @@
     <disable_collisions link1="$(arg prefix)_l/shoulder_link" link2="right_lift_top" reason="Never"/>
     <disable_collisions link1="$(arg prefix)_l/shoulder_link" link2="right_wheel_link" reason="Never"/>
     <disable_collisions link1="$(arg prefix)_l/shoulder_link" link2="top_module" reason="Never"/>
-    <disable_collisions link1="$(arg prefix)_l/tcp" link2="$(arg prefix)_l/wrist_1_link" reason="Never"/>
-    <disable_collisions link1="$(arg prefix)_l/tcp" link2="$(arg prefix)_l/wrist_2_link" reason="Never"/>
-    <disable_collisions link1="$(arg prefix)_l/tcp" link2="$(arg prefix)_l/wrist_3_link" reason="Adjacent"/>
-    <disable_collisions link1="$(arg prefix)_l/tcp" link2="back_laser_link" reason="Never"/>
-    <disable_collisions link1="$(arg prefix)_l/tcp" link2="bl_caster_rotation_link" reason="Never"/>
-    <disable_collisions link1="$(arg prefix)_l/tcp" link2="bl_caster_wheel_link" reason="Never"/>
-    <disable_collisions link1="$(arg prefix)_l/tcp" link2="br_caster_rotation_link" reason="Never"/>
-    <disable_collisions link1="$(arg prefix)_l/tcp" link2="br_caster_wheel_link" reason="Never"/>
     <disable_collisions link1="$(arg prefix)_l/upper_arm_link" link2="back_laser_link" reason="Never"/>
     <disable_collisions link1="$(arg prefix)_l/upper_arm_link" link2="base_link" reason="Never"/>
     <disable_collisions link1="$(arg prefix)_l/upper_arm_link" link2="bl_caster_rotation_link" reason="Never"/>
@@ -392,13 +392,6 @@
     <disable_collisions link1="$(arg prefix)_r/shoulder_link" link2="right_lift_top" reason="Never"/>
     <disable_collisions link1="$(arg prefix)_r/shoulder_link" link2="right_wheel_link" reason="Never"/>
     <disable_collisions link1="$(arg prefix)_r/shoulder_link" link2="top_module" reason="Never"/>
-    <disable_collisions link1="$(arg prefix)_r/tcp" link2="$(arg prefix)_r/wrist_1_link" reason="Never"/>
-    <disable_collisions link1="$(arg prefix)_r/tcp" link2="$(arg prefix)_r/wrist_2_link" reason="Never"/>
-    <disable_collisions link1="$(arg prefix)_r/tcp" link2="$(arg prefix)_r/wrist_3_link" reason="Adjacent"/>
-    <disable_collisions link1="$(arg prefix)_r/tcp" link2="back_laser_link" reason="Never"/>
-    <disable_collisions link1="$(arg prefix)_r/tcp" link2="bl_caster_rotation_link" reason="Never"/>
-    <disable_collisions link1="$(arg prefix)_r/tcp" link2="bl_caster_wheel_link" reason="Never"/>
-    <disable_collisions link1="$(arg prefix)_r/tcp" link2="br_caster_wheel_link" reason="Never"/>
     <disable_collisions link1="$(arg prefix)_r/upper_arm_link" link2="back_laser_link" reason="Never"/>
     <disable_collisions link1="$(arg prefix)_r/upper_arm_link" link2="base_link" reason="Never"/>
     <disable_collisions link1="$(arg prefix)_r/upper_arm_link" link2="bl_caster_rotation_link" reason="Never"/>
diff --git a/setup_full.sh b/setup_full.sh
index d287b397..b5c08f75 100755
--- a/setup_full.sh
+++ b/setup_full.sh
@@ -14,3 +14,21 @@ rosdep update
 rosdep install --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y
 catkin build
 source devel/setup.bash
+
+# install dependencies manually (this should usually be done through rosdep) 
+sudo apt install ros-noetic-costmap-2d
+sudo apt install ros-noetic-serial
+sudo apt install ros-noetic-nav-core
+sudo apt install ros-noetic-moveit-core
+sudo apt install ros-noetic-ur-client-library
+sudo apt install ros-noetic-moveit-ros-planning-interface
+sudo apt install ros-noetic-mbf-msgs
+sudo apt install ros-noetic-mir-actions
+sudo apt install ros-noetic-navfn
+sudo apt install ros-noetic-industrial-robot-status-interface
+sudo apt install ros-noetic-move-base-msgs
+sudo apt install ros-noetic-scaled-joint-trajectory-controller
+sudo apt install ros-noetic-rospy-message-converter
+sudo apt install ros-noetic-speed-scaling-interface
+sudo apt install ros-noetic-speed-scaling-state-controller
+sudo apt install ros-noetic-pass-through-controllers
\ No newline at end of file
diff --git a/tools/bauschaum_ee/CMakeLists.txt b/tools/bauschaum_ee/CMakeLists.txt
new file mode 100644
index 00000000..ccc360a2
--- /dev/null
+++ b/tools/bauschaum_ee/CMakeLists.txt
@@ -0,0 +1,202 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(bauschaum_ee)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES bauschaum_ee
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/bauschaum_ee.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/bauschaum_ee_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_bauschaum_ee.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/tools/bauschaum_ee/meshes/visual/bauschaum_ee.stl b/tools/bauschaum_ee/meshes/visual/bauschaum_ee.stl
new file mode 100644
index 00000000..644df98b
Binary files /dev/null and b/tools/bauschaum_ee/meshes/visual/bauschaum_ee.stl differ
diff --git a/tools/bauschaum_ee/package.xml b/tools/bauschaum_ee/package.xml
new file mode 100644
index 00000000..79fba8e8
--- /dev/null
+++ b/tools/bauschaum_ee/package.xml
@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>bauschaum_ee</name>
+  <version>0.0.0</version>
+  <description>The bauschaum_ee package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="rosmatch@todo.todo">rosmatch</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/bauschaum_ee</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
diff --git a/tools/bauschaum_ee/urdf/bauschaum_ee.urdf.xacro b/tools/bauschaum_ee/urdf/bauschaum_ee.urdf.xacro
new file mode 100644
index 00000000..9c1e58d5
--- /dev/null
+++ b/tools/bauschaum_ee/urdf/bauschaum_ee.urdf.xacro
@@ -0,0 +1,90 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+	<xacro:include filename="$(find mir_description)/urdf/general/common_properties.urdf.xacro" />
+
+	<xacro:macro name="bauschaum_ee" params="tool_link tf_prefix">
+		
+		<link name="${tf_prefix}/gripper">
+			<xacro:cylinder_inertial mass="1.5" radius="0.32" length="0.128">
+				<origin xyz="0 0 0.0" rpy="0 0 0" />
+			</xacro:cylinder_inertial>
+		</link>
+
+		<joint name="${tf_prefix}/gripper_joint" type="fixed">
+			<origin xyz="0.5 -0.13 0.0" rpy="${-pi * 0.5} ${pi * 0.0} ${pi * 0.0}" />
+			<parent link="${tool_link}" />
+			<child link="${tf_prefix}/gripper" />
+			<axis xyz="0 0 1" />
+			<limit effort="1000" lower="-0.0001" upper="0.0001" velocity="10"/>
+		</joint>
+
+		<link name="${tf_prefix}/tcp">
+			<xacro:cylinder_inertial mass="0.01" radius="0.32" length="0.128">
+				<origin xyz="0 0 0.0" rpy="0 0 0" />
+			</xacro:cylinder_inertial>
+			<visual>
+				<origin xyz="0.0 0.0 0" rpy="0 ${pi * 0.5} ${pi * 0.0}" />
+				<geometry>
+					<mesh filename="package://bauschaum_ee/meshes/visual/bauschaum_ee.stl" scale="0.001 0.001 0.001"/>
+				</geometry>
+				<xacro:insert_block name="material_white" />
+			</visual>
+		</link>
+
+		<gazebo reference="${tf_prefix}/tcp">
+			<material>Gazebo/DarkGrey</material>
+		</gazebo>
+
+		<!-- the ft_sensor plugin will not work with fixed joints. This is why a prismatic joint with very small limits is added -->
+		<joint name="${tf_prefix}/tcp_joint" type="prismatic">
+			<origin xyz="-0.5 0.0 0.13" rpy="0 0 0" />
+			<parent link="${tf_prefix}/gripper" />
+			<child link="${tf_prefix}/tcp" />
+			<axis xyz="0 0 1" />
+   			<limit effort="1000" lower="-0.0001" upper="0.0001" velocity="10"/>
+		</joint>
+		
+
+		<!-- Gazebo Plugin for Line Laser -->
+		<gazebo reference="${tf_prefix}/gripper">
+			<sensor type="ray" name="line_laser">
+			<pose>0 0 0 0.0 0 0</pose>
+			<visualize>true</visualize>
+			<update_rate>20.0</update_rate>
+			<ray>
+				<scan>
+				<!-- Horizontal range parameters -->
+				<horizontal>
+					<samples>1000</samples> <!-- Number of laser beams -->
+					<resolution>1</resolution>
+					<min_angle>-0.27317277996474193</min_angle> <!-- Start angle in radians -->
+					<max_angle>0.27317277996474193</max_angle> <!-- End angle in radians -->
+				</horizontal>
+				<!-- Vertical range parameters (set min and max equal for a line) -->
+				<vertical>
+					<samples>1</samples> <!-- Single vertical beam -->
+					<min_angle>0.0</min_angle>
+					<max_angle>0.0</max_angle>
+				</vertical>
+				</scan>
+				<range>
+				<min>0.285</min> <!-- Minimum range in meters -->
+				<max>0.600</max> <!-- Maximum range in meters -->
+				<resolution>0.0001707329874779637</resolution> <!-- Range resolution -->
+				</range>
+				<noise>
+				<type>gaussian</type>
+				<mean>0.00005</mean>
+				<stddev>0.0005</stddev>
+				</noise>
+			</ray>
+			<plugin name="gazebo_ros_laser" filename="libgazebo_ros_laser.so">
+				<topicName>${tf_prefix}/line_laser/scan</topicName> <!-- ROS topic -->
+				<frameName>mur620a/${tf_prefix}/gripper</frameName> <!-- Frame for the laser -->
+			</plugin>
+			</sensor>
+		</gazebo>
+		
+	</xacro:macro>
+
+</robot>
\ No newline at end of file
diff --git a/tools/keyence_lj_x/urdf/keyence_lj_x.urdf.xacro b/tools/keyence_lj_x/urdf/keyence_lj_x.urdf.xacro
index 1522db2c..e8ebed92 100644
--- a/tools/keyence_lj_x/urdf/keyence_lj_x.urdf.xacro
+++ b/tools/keyence_lj_x/urdf/keyence_lj_x.urdf.xacro
@@ -11,7 +11,7 @@
 		</link>
 
 		<joint name="${tf_prefix}/gripper_joint" type="fixed">
-			<origin xyz="0 0 0.0" rpy="${-pi * 0.5} ${pi * 0.0} ${pi * 0.0}" />
+			<origin xyz="0.05 0 0.0" rpy="${-pi * 0.0} ${pi * 0.0} ${pi * 0.0}" />
 			<parent link="${tool_link}" />
 			<child link="${tf_prefix}/gripper" />
 			<axis xyz="0 0 1" />
diff --git a/ur/ur_controllers_match/scripts/direct_kinematics_publisher.py b/ur/ur_controllers_match/scripts/direct_kinematics_publisher.py
index 0a7e5dce..f50a9719 100755
--- a/ur/ur_controllers_match/scripts/direct_kinematics_publisher.py
+++ b/ur/ur_controllers_match/scripts/direct_kinematics_publisher.py
@@ -13,6 +13,10 @@ def __init__(self):
         # Initialize the node
         rospy.init_node('direct_kinematics_publisher', anonymous=True)
         self.load_params()
+        # get current namespace
+        self.ns = rospy.get_namespace()
+        self.ns = self.ns[1:]
+
         self.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
         self.DH_params = [[0,0,0.1807,pi/2],[0,-0.6127,0,0],[0,-0.57155,0,0],[0,0,0.17415,pi/2],[0,0,0.11985,-pi/2],[0,0,0.11655,0]]
         self.q = [0,0,0,0,0,0]
@@ -39,7 +43,7 @@ def joint_state_callback(self,JointState):
         self.tcp_pose.pose.orientation.z = q[2]
         self.tcp_pose.pose.orientation.w = q[3]
         self.tcp_pose.header.stamp = rospy.Time.now()
-        self.tcp_pose.header.frame_id = "base_link"
+        self.tcp_pose.header.frame_id = self.ns+"base_link"
         self.pose_pub.publish(self.tcp_pose)
 
 
diff --git a/ur/ur_controllers_match/src/twist_controller.cpp b/ur/ur_controllers_match/src/twist_controller.cpp
index d128b36d..de681ecd 100644
--- a/ur/ur_controllers_match/src/twist_controller.cpp
+++ b/ur/ur_controllers_match/src/twist_controller.cpp
@@ -39,6 +39,19 @@ class TwistController
 
         joint_model_group = move_group->getCurrentState(5.0)->getJointModelGroup(PLANNING_GROUP);
 
+        // Get base link:
+        ros::V_string link_names = joint_model_group->getLinkModelNames();
+        auto base_link = link_names[0];
+        ROS_INFO("Base link of joint_model_group: %s", base_link.c_str());
+
+        // Get rotation between base_link and move_group->getPlanningFrame():
+        auto tf_base = move_group->getCurrentState(5.0)->getGlobalLinkTransform(base_link);
+        tf_rotation = tf_base.rotation();
+        tf_rotation.block(0,0,2,2) = -tf_rotation.block(0,0,2,2); // rotation of base_link to base_link_inertia
+        Eigen::Quaternion<double> q(tf_rotation);
+        q_tf_rotation = tf2::Quaternion(q.x(), q.y(), q.z(), q.w());
+        ROS_INFO_STREAM("Rotation between base_link and planning_frame: " << tf_rotation);
+
 
         // Set up a publisher for joint velocity commands
         joint_vel_pub = nh.advertise<std_msgs::Float64MultiArray>(ur_prefix+"joint_group_vel_controller/command", 1);
@@ -55,6 +68,8 @@ class TwistController
         // Extract the Cartesian twist command from the message
         geometry_msgs::Twist twist = *twist_msg;
         twistVector = Eigen::VectorXd::Map(&twist.linear.x, 6);
+        twistVector.block(0,0,3,1) = tf_rotation*twistVector.block(0,0,3,1);
+        twistVector.block(3,0,3,1) = tf_rotation*twistVector.block(3,0,3,1);
 
     }
 
@@ -92,6 +107,8 @@ class TwistController
     std::unique_ptr<moveit::planning_interface::MoveGroupInterface> move_group;
     std::string PLANNING_GROUP;
     const robot_state::JointModelGroup* joint_model_group;
+    Eigen::Matrix3d tf_rotation;
+    tf2::Quaternion q_tf_rotation;
     Eigen::VectorXd twistVector;
 };
 
diff --git a/ur/ur_utilities/launch/move_UR_to_home_pose.launch b/ur/ur_utilities/launch/move_UR_to_home_pose.launch
index 262d7479..2b3db48f 100644
--- a/ur/ur_utilities/launch/move_UR_to_home_pose.launch
+++ b/ur/ur_utilities/launch/move_UR_to_home_pose.launch
@@ -7,9 +7,9 @@
     <arg name="move_group_name" default="UR_arm_r"/>
 
     <!-- enable UR -->
-    <node name="UR_enable" pkg="ur_utilities" type="UR_enable.py" output="screen">
+    <!-- <node name="UR_enable" pkg="ur_utilities" type="UR_enable.py" output="screen">
         <param name="ur_hardware_interface_topic" value="/$(arg tf_prefix)/$(arg UR_prefix)/ur_hardware_interface/set_mode" />
-    </node>
+    </node> -->
 
     <node name="move_UR_to_home_position" pkg="ur_utilities" type="move_UR_to_home_position.py" output="screen">
         <param name="UR_prefix" value="$(arg UR_prefix)" />