diff --git a/CMakeLists.txt b/CMakeLists.txt
index b258306..d06050e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(rewd_controllers)
 
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17")
 
 include_directories("include")
 
@@ -66,6 +66,7 @@ add_library("${PROJECT_NAME}" SHARED
   src/MoveUntilTouchController.cpp
   src/MoveUntilTouchTopicController.cpp
   src/FTThresholdClient.cpp
+  src/MoveUntilTouchCartVelocityController.cpp
   src/FTThresholdServer.cpp
 )
 target_link_libraries("${PROJECT_NAME}"
diff --git a/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp b/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp
new file mode 100644
index 0000000..64217c4
--- /dev/null
+++ b/include/rewd_controllers/MoveUntilTouchCartVelocityController.hpp
@@ -0,0 +1,116 @@
+#ifndef REWD_CONTROLLERS_MOVEUNTILTOUCHCARTVELOCITY_HPP_
+#define REWD_CONTROLLERS_MOVEUNTILTOUCHCARTVELOCITY_HPP_
+
+#include "helpers.hpp"
+#include <actionlib/server/action_server.h>
+#include <atomic>
+#include <controller_interface/multi_interface_controller.h>
+#include <dart/dynamics/dynamics.hpp>
+#include <memory>
+#include <realtime_tools/realtime_buffer.h>
+#include <ros/node_handle.h>
+
+#include <pr_control_msgs/SetCartesianVelocityAction.h>
+#include <pr_hardware_interfaces/CartesianVelocityInterface.h>
+
+#include <rewd_controllers/FTThresholdServer.hpp>
+
+namespace rewd_controllers {
+class MoveUntilTouchCartVelocityController
+    : public controller_interface::MultiInterfaceController<
+          hardware_interface::VelocityJointInterface,
+          hardware_interface::JointStateInterface,
+          hardware_interface::JointModeInterface> {
+public:
+  MoveUntilTouchCartVelocityController();
+  virtual ~MoveUntilTouchCartVelocityController();
+
+  /** \brief The init function is called to initialize the controller from a
+   * non-realtime thread with a pointer to the hardware interface, itself,
+   * instead of a pointer to a RobotHW.
+   *
+   * \param robot The specific hardware interface used by this controller.
+   *
+   * \param n A NodeHandle in the namespace from which the controller
+   * should read its configuration, and where it should set up its ROS
+   * interface.
+   *
+   * \returns True if initialization was successful and the controller
+   * is ready to be started.
+   */
+  bool init(hardware_interface::RobotHW *robot, ros::NodeHandle &n);
+
+  /*!
+   * \brief Issues commands to the joint. Should be called at regular intervals
+   */
+  void update(const ros::Time &time, const ros::Duration &period);
+
+  /** \brief This is called from within the realtime thread just before the
+   * first call to \ref update
+   *
+   * \param time The current time
+   */
+  void starting(const ros::Time &time) override;
+  void stopping(const ros::Time &time) override;
+
+protected:
+  using Action = pr_control_msgs::SetCartesianVelocityAction;
+  using ActionServer = actionlib::ActionServer<Action>;
+  using GoalHandle = ActionServer::GoalHandle;
+
+  using Result = pr_control_msgs::SetCartesianVelocityResult;
+
+  using JointModes = hardware_interface::JointCommandModes;
+
+private:
+  // \brief Force-Torque Thresholding Server
+  std::shared_ptr<FTThresholdServer> mFTThresholdServer;
+  bool mUseFT;
+  
+  /** \brief Actionlib callback to accept new Goal.
+   */
+  void goalCallback(GoalHandle goalHandle);
+
+  /** \brief Actionlib callback to cancel previously accepted goal.
+   */
+  void cancelCallback(GoalHandle goalHandle);
+
+  /** \brief Contains all data needed to execute the currently
+   * requested velocity. Shared between real time and non-real
+   * time threads.
+   *
+   * This structure must be used AS A WHOLE, and reassigned
+   * in its entirety. Assigning to individual components
+   * is UNSAFE with the exception of the mCompleted atomic.
+   */
+  struct CartVelContext {
+    ros::Time mStartTime;
+    ros::Duration mDuration;
+    Eigen::VectorXd mDesiredVelocity;
+    GoalHandle mGoalHandle;
+    std::atomic_bool mCompleted;
+  };
+  using CartVelContextPtr = std::shared_ptr<CartVelContext>;
+
+  // For position feedback:
+  dart::dynamics::SkeletonPtr mSkeleton;
+  std::unique_ptr<SkeletonJointStateUpdater> mSkeletonUpdater;
+  dart::dynamics::MetaSkeletonPtr mControlledSkeleton;
+  std::string mEEName;
+
+  // Action server variables
+  std::unique_ptr<ActionServer> mActionServer;
+
+  // TODO: It would be better to use std::atomic<std::shared_ptr<T>> here.
+  // However, this will not be implemented until C++20.
+  realtime_tools::RealtimeBox<CartVelContextPtr> mCurrentCartVel;
+
+  // For communication with robot
+  std::vector<hardware_interface::JointHandle> mControlledJointHandles;
+  hardware_interface::JointModeHandle mJointModeHandle;
+  JointModes lastMode;
+};
+
+} // namespace rewd_controllers
+
+#endif
diff --git a/include/rewd_controllers/MultiInterfaceController.hpp b/include/rewd_controllers/MultiInterfaceController.hpp
index ee8dc5c..d091338 100644
--- a/include/rewd_controllers/MultiInterfaceController.hpp
+++ b/include/rewd_controllers/MultiInterfaceController.hpp
@@ -201,7 +201,7 @@ class MultiInterfaceController : public ControllerBase {
    */
   MultiInterfaceController(bool allow_optional_interfaces = false)
       : allow_optional_interfaces_(allow_optional_interfaces) {
-    state_ = CONSTRUCTED;
+    state_ = ControllerState::CONSTRUCTED;
   }
 
   virtual ~MultiInterfaceController() {}
@@ -300,7 +300,7 @@ class MultiInterfaceController : public ControllerBase {
                            ros::NodeHandle &controller_nh,
                            ClaimedResources &claimed_resources) {
     // check if construction finished cleanly
-    if (state_ != CONSTRUCTED) {
+    if (state_ != ControllerState::CONSTRUCTED) {
       ROS_ERROR("Cannot initialize this controller because it failed to be "
                 "constructed");
       return false;
@@ -333,7 +333,7 @@ class MultiInterfaceController : public ControllerBase {
     // the controller is done when the controller is start()ed
 
     // initialization successful
-    state_ = INITIALIZED;
+    state_ = ControllerState::INITIALIZED;
     return true;
   }
 
diff --git a/include/rewd_controllers/helpers.hpp b/include/rewd_controllers/helpers.hpp
index 68af18a..00c39ec 100644
--- a/include/rewd_controllers/helpers.hpp
+++ b/include/rewd_controllers/helpers.hpp
@@ -4,6 +4,7 @@
 #include "JointAdapterFactory.hpp"
 #include <dart/dynamics/dynamics.hpp>
 #include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/joint_mode_interface.h>
 #include <hardware_interface/joint_state_interface.h>
 #include <hardware_interface/robot_hw.h>
 #include <ros/node_handle.h>
@@ -12,6 +13,8 @@
 #include <typeinfo>
 #include <vector>
 
+#include <pr_hardware_interfaces/CartesianVelocityInterface.h>
+
 namespace rewd_controllers {
 
 //=============================================================================
diff --git a/rewd_controllers_plugins.xml b/rewd_controllers_plugins.xml
index 3a02b25..d6f5574 100644
--- a/rewd_controllers_plugins.xml
+++ b/rewd_controllers_plugins.xml
@@ -68,4 +68,11 @@
       SetForceTorqueThreshold service is exceeded.
     </description>
   </class>
+  <class name="rewd_controllers/MoveUntilTouchCartVelocityController"
+         type="rewd_controllers::MoveUntilTouchCartVelocityController"
+         base_class_type="controller_interface::ControllerBase">
+    <description>
+      TODO
+    </description>
+  </class>
 </library>
diff --git a/src/MoveUntilTouchCartVelocityController.cpp b/src/MoveUntilTouchCartVelocityController.cpp
new file mode 100644
index 0000000..c486128
--- /dev/null
+++ b/src/MoveUntilTouchCartVelocityController.cpp
@@ -0,0 +1,301 @@
+#include <rewd_controllers/MoveUntilTouchCartVelocityController.hpp>
+
+#include <dart/dynamics/dynamics.hpp>
+#include <dart/utils/urdf/DartLoader.hpp>
+#include <hardware_interface/hardware_interface.h>
+#include <pluginlib/class_list_macros.h>
+
+#include <pr_control_msgs/SetCartesianVelocityAction.h>
+
+#define SE3_SIZE 6
+
+namespace rewd_controllers {
+namespace {
+//=============================================================================
+std::vector<double> toVector(const Eigen::VectorXd &input) {
+  return std::vector<double>{input.data(), input.data() + input.size()};
+}
+
+} // namespace
+
+//=============================================================================
+MoveUntilTouchCartVelocityController::MoveUntilTouchCartVelocityController()
+    : MultiInterfaceController(true) // allow_optional_interfaces
+{
+  mUseFT = false;
+}
+
+//=============================================================================
+MoveUntilTouchCartVelocityController::~MoveUntilTouchCartVelocityController() {}
+
+//=============================================================================
+bool MoveUntilTouchCartVelocityController::init(
+    hardware_interface::RobotHW *robot, ros::NodeHandle &n) {
+  using hardware_interface::JointModeInterface;
+  using hardware_interface::JointStateInterface;
+  using hardware_interface::VelocityJointInterface;
+
+  // Enable force/torque functionality if requested
+  mUseFT = false;
+  n.getParam("enableFT", mUseFT);
+
+  if(mUseFT) {
+    // load name of force/torque sensor handle from paramter
+    std::string ft_wrench_name;
+    if (!n.getParam("forcetorque_wrench_name", ft_wrench_name)) {
+      ROS_ERROR("Failed to load 'forcetorque_wrench_name' parameter.");
+      return false;
+    }
+    // load name of force/torque tare handle from paramter
+    std::string ft_tare_name;
+    if (!n.getParam("forcetorque_tare_name", ft_tare_name)) {
+      ROS_ERROR("Failed to load 'forcetorque_tare_name' parameter.");
+      return false;
+    }
+
+    // load force/torque saturation limits from parameter
+    double forceLimit = 0.0;
+    if (!n.getParam("sensor_force_limit", forceLimit)) {
+      ROS_ERROR("Failed to load 'sensor_force_limit' parameter.");
+      return false;
+    }
+    double torqueLimit = 0.0;
+    if (!n.getParam("sensor_torque_limit", torqueLimit)) {
+      ROS_ERROR("Failed to load 'sensor_torque_limit' parameter.");
+      return false;
+    }
+    if (forceLimit < 0) {
+      ROS_ERROR("sensor_force_limit must be positive or zero");
+      return false;
+    }
+    if (torqueLimit < 0) {
+      ROS_ERROR("sensor_torque_limit must be positive or zero");
+      return false;
+    }
+
+    // Init FT Threshold Server
+    mFTThresholdServer.reset(new FTThresholdServer{n,
+            ft_wrench_name,
+            ft_tare_name,
+            forceLimit,
+            torqueLimit});
+  }
+
+  // Load the URDF as a Skeleton.
+  mSkeleton = loadRobotFromParameter(n, "robot_description_parameter");
+  if (!mSkeleton)
+    return false;
+
+  // Have skeleton update from joint state interface
+  const auto jointStateInterface = robot->get<JointStateInterface>();
+  if (!jointStateInterface) {
+    ROS_ERROR("Unable to get JointStateInterface from RobotHW instance.");
+    return false;
+  }
+
+  mSkeletonUpdater.reset(
+      new SkeletonJointStateUpdater{mSkeleton, jointStateInterface});
+
+  // Build up the list of controlled joints.
+  const auto jointParameters = loadJointsFromParameter(n, "joints", "velocity");
+  if (jointParameters.empty())
+    return false;
+
+  ROS_INFO_STREAM("Controlling " << jointParameters.size() << " joints:");
+  for (const auto &param : jointParameters) {
+    ROS_INFO_STREAM("- " << param.mName << " (type: " << param.mType << ")");
+
+    if (param.mType != "velocity") {
+      ROS_ERROR_STREAM("Joint '"
+                       << param.mName
+                       << "' is not velocity-controlled and cannot be "
+                          "used in a cartesian velocity controller");
+      return false;
+    }
+  }
+
+  // Extract the subset of the Skeleton that is being controlled.
+  mControlledSkeleton =
+      getControlledMetaSkeleton(mSkeleton, jointParameters, "Controlled");
+  if (!mControlledSkeleton) {
+    return false;
+  }
+
+  const auto numControlledDofs = mControlledSkeleton->getNumDofs();
+  mControlledJointHandles.resize(numControlledDofs);
+
+  // Load control interfaces and handles
+  const auto jointModeInterface = robot->get<JointModeInterface>();
+  if (!jointModeInterface) {
+    ROS_ERROR("Unable to get JointModeInterface from RobotHW instance.");
+    return false;
+  }
+
+  try {
+    mJointModeHandle = jointModeInterface->getHandle("joint_mode");
+  } catch (const hardware_interface::HardwareInterfaceException &e) {
+    ROS_ERROR_STREAM("Unable to get joint mode interface for robot");
+    return false;
+  }
+  const auto velocityJointInterface = robot->get<VelocityJointInterface>();
+  if (!velocityJointInterface) {
+    ROS_ERROR("Unable to get VelocityJointInterface from RobotHW instance.");
+    return false;
+  }
+
+  for (size_t idof = 0; idof < numControlledDofs; ++idof) {
+    const auto dofName = mControlledSkeleton->getDof(idof)->getName();
+    try {
+      auto handle = velocityJointInterface->getHandle(dofName);
+      mControlledJointHandles[idof] = handle;
+    } catch (const hardware_interface::HardwareInterfaceException &e) {
+      ROS_ERROR_STREAM(
+          "Unable to get interface of type 'VelocityJointInterface' for joint '"
+          << dofName << "'.");
+      return false;
+    }
+  }
+
+  // init local vars
+  mCurrentCartVel.set(nullptr);
+  if (!n.getParam("ee", mEEName)) {
+    ROS_ERROR_STREAM("Parameter '" << n.getNamespace()
+                                   << "/ee' is required.");
+  }
+
+  // Start the action server. This must be last.
+  using std::placeholders::_1;
+  mActionServer.reset(new actionlib::ActionServer<Action>{
+      n, "cart_velocity",
+      std::bind(&MoveUntilTouchCartVelocityController::goalCallback, this, _1),
+      std::bind(&MoveUntilTouchCartVelocityController::cancelCallback, this,
+                _1),
+      false});
+  mActionServer->start();
+
+  ROS_INFO("MoveUntilTouchCartVelocityController initialized successfully");
+  return true;
+}
+
+//=============================================================================
+void MoveUntilTouchCartVelocityController::starting(const ros::Time &time) {
+  // Set Joint Mode to Other
+  lastMode = mJointModeHandle.getMode();
+  mJointModeHandle.setMode(JointModes::MODE_VELOCITY);
+
+  // start FTThresholdServer
+  mFTThresholdServer->start();
+}
+
+//=============================================================================
+void MoveUntilTouchCartVelocityController::stopping(const ros::Time &time) {
+  // Return joint mode to what it was before
+  mJointModeHandle.setMode(lastMode);
+
+  // stop FTThresholdServer
+  mFTThresholdServer->stop();
+}
+
+//=============================================================================
+void MoveUntilTouchCartVelocityController::update(const ros::Time &time,
+                                                  const ros::Duration &period) {
+  mSkeletonUpdater->update();
+
+  Eigen::VectorXd setVelocity;
+  setVelocity.resize(SE3_SIZE);
+  setVelocity.setZero();
+
+  // Check FT Threshold
+  bool ftThresholdTripped = false;
+  std::string message = "";
+  if(mUseFT) {
+    ftThresholdTripped = mFTThresholdServer->shouldStopExecution(message);
+  }
+
+  // Load current command
+  std::shared_ptr<CartVelContext> context;
+  mCurrentCartVel.get(context);
+
+  if (context && !context->mCompleted.load()) {
+    // check duration
+    ros::Duration runTime = time - context->mStartTime;
+    if (runTime >= context->mDuration) {
+      // Successful run
+      Result result;
+      result.error_code = Result::SUCCESSFUL;
+      result.error_string = "";
+      context->mGoalHandle.setSucceeded(result);
+      context->mCompleted.store(true);
+    } else if (ftThresholdTripped) {
+      // Aborted run
+      Result result;
+      result.error_code = Result::FORCE_THRESH;
+      result.error_string = message;
+      context->mGoalHandle.setAborted(result);
+      context->mCompleted.store(true);
+    } else {
+      setVelocity = context->mDesiredVelocity;
+    }
+  }
+
+  // Execute velocity command
+  /* Get full (with orientation) Jacobian of our end-effector */
+  Eigen::VectorXd jointVels(mControlledSkeleton->getNumDofs());
+  jointVels.setZero();
+  if (setVelocity.norm() != 0) {
+    Eigen::MatrixXd J = mSkeleton->getBodyNode(mEEName)->getWorldJacobian();
+    Eigen::MatrixXd JtJ = J.transpose() * J;
+    if (JtJ.determinant() != 0) {
+      jointVels = JtJ.inverse() * J.transpose() * setVelocity;
+    } else {
+      ROS_ERROR_STREAM("Error: at singularity. Cartesian control impossible.");
+    }
+  }
+
+  for (size_t idof = 0; idof < mControlledJointHandles.size(); ++idof) {
+    auto jointHandle = mControlledJointHandles[idof];
+    jointHandle.setCommand(jointVels(idof));
+  }
+}
+
+//=============================================================================
+void MoveUntilTouchCartVelocityController::goalCallback(GoalHandle goalHandle) {
+  const auto goal = goalHandle.getGoal();
+  ROS_INFO_STREAM("Received cartesian velocity '" << goalHandle.getGoalID().id
+                                                  << "'.");
+
+  // Setup the new trajectory.
+  const auto newContext = std::make_shared<CartVelContext>();
+  newContext->mStartTime = ros::Time::now();
+  newContext->mGoalHandle = goalHandle;
+  newContext->mDuration = goal->exec_time;
+  newContext->mDesiredVelocity.resize(SE3_SIZE);
+  newContext->mCompleted.store(false);
+
+  // Copy over velocity data
+  newContext->mDesiredVelocity(0) = goal->command.angular.x;
+  newContext->mDesiredVelocity(1) = goal->command.angular.y;
+  newContext->mDesiredVelocity(2) = goal->command.angular.z;
+  newContext->mDesiredVelocity(3) = goal->command.linear.x;
+  newContext->mDesiredVelocity(4) = goal->command.linear.y;
+  newContext->mDesiredVelocity(5) = goal->command.linear.z;
+
+  // TODO: add velocity checks
+
+  // We've accepted the goal
+  newContext->mGoalHandle.setAccepted();
+  mCurrentCartVel.set(newContext);
+}
+
+//=============================================================================
+void MoveUntilTouchCartVelocityController::cancelCallback(
+    GoalHandle goalHandle) {
+  ROS_INFO_STREAM("Requesting cancelation of velocity '"
+                  << goalHandle.getGoalID().id << "'.");
+  mCurrentCartVel.set(nullptr);
+}
+
+} // namespace rewd_controllers
+
+PLUGINLIB_EXPORT_CLASS(rewd_controllers::MoveUntilTouchCartVelocityController,
+                       controller_interface::ControllerBase)