diff --git a/launch/ur10_ros_control.launch b/launch/ur10_ros_control.launch
index 733ccfe..cba1407 100644
--- a/launch/ur10_ros_control.launch
+++ b/launch/ur10_ros_control.launch
@@ -4,11 +4,12 @@
   <arg name="debug" default="false" />
   <arg unless="$(arg debug)" name="launch_prefix" value="" />
   <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
+  
   <arg name="robot_ip"/>
   <arg name="limited" default="false"/>
   <arg name="min_payload"  default="0.0"/>
   <arg name="max_payload"  default="10.0"/>
-
+  <arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
   <!-- robot model -->
   <include file="$(find ur_description)/launch/ur10_upload.launch">
     <arg name="limited" value="$(arg limited)"/>
@@ -22,6 +23,7 @@
 	    <param name="min_payload" type="double" value="$(arg min_payload)"/>
 	    <param name="max_payload" type="double" value="$(arg max_payload)"/>
 	    <param name="max_velocity" type="double" value="$(arg max_velocity)"/>
+	    <param name="use_ros_control" type="bool" value="True"/>
     </node>
 
     <!-- Load controller settings -->
@@ -29,7 +31,7 @@
 
     <!-- Load controller manager -->
     <node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
-	  output="screen" args="spawn joint_state_controller force_torque_sensor_controller position_trajectory_controller" />
+	  output="screen" args="spawn joint_state_controller force_torque_sensor_controller position_trajectory_controller" /> 
 
     <!-- Convert joint states to /tf tranforms -->
     <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
diff --git a/launch/ur5_ros_control.launch b/launch/ur5_ros_control.launch
index 050553b..0069cd0 100644
--- a/launch/ur5_ros_control.launch
+++ b/launch/ur5_ros_control.launch
@@ -4,6 +4,7 @@
   <arg name="debug" default="false" />
   <arg unless="$(arg debug)" name="launch_prefix" value="" />
   <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
+  
   <arg name="robot_ip"/>
   <arg name="limited" default="false"/>
   <arg name="min_payload"  default="0.0"/>
@@ -22,14 +23,15 @@
 	    <param name="min_payload" type="double" value="$(arg min_payload)"/>
 	    <param name="max_payload" type="double" value="$(arg max_payload)"/>
 	    <param name="max_velocity" type="double" value="$(arg max_velocity)"/>
+	    <param name="use_ros_control" type="bool" value="True"/>
     </node>
 
     <!-- Load controller settings -->
     <rosparam file="$(find ur_modern_driver)/config/ur5_controllers.yaml" command="load"/>
 
     <!-- Load controller manager -->
-    <!-- <node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
-	  output="screen" args="spawn joint_state_controller force_torque_sensor_controller position_trajectory_controller" /> -->
+    <node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
+	  output="screen" args="spawn joint_state_controller force_torque_sensor_controller position_trajectory_controller" /> 
 
     <!-- Convert joint states to /tf tranforms -->
     <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp
index e20c77e..9e448ce 100644
--- a/src/ur_ros_wrapper.cpp
+++ b/src/ur_ros_wrapper.cpp
@@ -55,10 +55,11 @@ protected:
 	ros::ServiceServer payload_srv_;
 	std::thread* rt_publish_thread_;
 	std::thread* mb_publish_thread_;
-	std::thread* ros_control_thread_;
 	double io_flag_delay_;
 	double max_velocity_;
 	std::vector<double> joint_offsets_;
+	bool use_ros_control_;
+	std::thread* ros_control_thread_;
 	boost::shared_ptr<ros_control_ur::UrHardwareInterface> hardware_interface_;
 	boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
 
@@ -82,12 +83,16 @@ public:
 		joint_names.push_back(joint_prefix + "wrist_3_joint");
 		robot_.setJointNames(joint_names);
 
-		hardware_interface_.reset(
-				new ros_control_ur::UrHardwareInterface(nh_, &robot_));
-		controller_manager_.reset(
-				new controller_manager::ControllerManager(
-						hardware_interface_.get(), nh_));
+		use_ros_control_ = false;
+		ros::param::get("~use_ros_control", use_ros_control_);
 
+		if (use_ros_control_) {
+			hardware_interface_.reset(
+					new ros_control_ur::UrHardwareInterface(nh_, &robot_));
+			controller_manager_.reset(
+					new controller_manager::ControllerManager(
+							hardware_interface_.get(), nh_));
+		}
 		//Using a very high value in order to not limit execution of trajectories being sent from MoveIt!
 		max_velocity_ = 10.;
 		if (ros::param::get("~max_velocity", max_velocity_)) {
@@ -95,31 +100,43 @@ public:
 					max_velocity_);
 		}
 
-		{ //Bounds for SetPayload service
-		  //Using a very conservative value as it should be set through the parameter server
-			double min_payload = 0.;
-			double max_payload = 1.;
-			if (ros::param::get("~min_payload", min_payload)) {
-				ROS_DEBUG("Min payload set to: %f [kg]", min_payload);
-			}
-			if (ros::param::get("~max_payload", max_payload)) {
-				ROS_DEBUG("Max payload set to: %f [kg]", max_payload);
-			}
-			robot_.setMinPayload(min_payload);
-			robot_.setMaxPayload(max_payload);
-			ROS_DEBUG("Bounds for set_payload service calls: [%f, %f]",
-					min_payload, max_payload);
+		//Bounds for SetPayload service
+		//Using a very conservative value as it should be set through the parameter server
+		double min_payload = 0.;
+		double max_payload = 1.;
+		if (ros::param::get("~min_payload", min_payload)) {
+			ROS_DEBUG("Min payload set to: %f [kg]", min_payload);
 		}
+		if (ros::param::get("~max_payload", max_payload)) {
+			ROS_DEBUG("Max payload set to: %f [kg]", max_payload);
+		}
+		robot_.setMinPayload(min_payload);
+		robot_.setMaxPayload(max_payload);
+		ROS_DEBUG("Bounds for set_payload service calls: [%f, %f]", min_payload,
+				max_payload);
 
 		if (robot_.start()) {
+			if (use_ros_control_) {
+				ros_control_thread_ = new std::thread(
+						boost::bind(&RosWrapper::rosControlLoop, this));
+				ROS_DEBUG(
+						"The control thread for this driver has been started");
+			} else {
+				//register the goal and feedback callbacks
+				as_.registerGoalCallback(
+						boost::bind(&RosWrapper::goalCB, this));
+				as_.registerPreemptCallback(
+						boost::bind(&RosWrapper::preemptCB, this));
 
-			//register the goal and feedback callbacks
-			//as_.registerGoalCallback(boost::bind(&RosWrapper::goalCB, this));
-			//as_.registerPreemptCallback(boost::bind(&RosWrapper::preemptCB, this));
+				as_.start();
 
-			//as_.start();
-
-			//subscribe to the data topic of interest
+				//subscribe to the data topic of interest
+				rt_publish_thread_ = new std::thread(
+						boost::bind(&RosWrapper::publishRTMsg, this));
+				ROS_DEBUG("The action server for this driver has been started");
+			}
+			mb_publish_thread_ = new std::thread(
+					boost::bind(&RosWrapper::publishMbMsg, this));
 			speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1,
 					&RosWrapper::speedInterface, this);
 			urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1,
@@ -129,14 +146,6 @@ public:
 					&RosWrapper::setIO, this);
 			payload_srv_ = nh_.advertiseService("ur_driver/set_payload",
 					&RosWrapper::setPayload, this);
-
-			ros_control_thread_ = new std::thread(
-					boost::bind(&RosWrapper::rosControlLoop, this));
-			/*rt_publish_thread_ = new std::thread(
-			 boost::bind(&RosWrapper::publishRTMsg, this)); */
-			mb_publish_thread_ = new std::thread(
-					boost::bind(&RosWrapper::publishMbMsg, this));
-			ROS_DEBUG("The action server for this driver has been started");
 		}
 	}