Added cartesian velocity control function

include/libada/Ada.hpp

@@ -30,6 +30,9 @@
 #include <dart/dart.hpp>
 #include <ros/ros.h>
 
+#include <pr_control_msgs/SetCartesianVelocityAction.h>
+#include <actionlib/client/simple_action_client.h>
+
 #include "libada/AdaHand.hpp"
 
 namespace ada {
@@ -54,6 +57,17 @@ struct KunzParams : aikido::planner::kunzretimer::KunzRetimer::Params
   }
 };
 
+// Enum for Cartesian Velocity Return Result
+enum CartVelocityResult
+{
+  kCVR_SUCCESS,
+  kCVR_INVALID,
+  kCVR_FORCE,
+  kCVR_CANCELLED,
+  kCVR_TIMEOUT,
+  kCVR_UNKNOWN
+};
+
 class Ada final : public aikido::robot::Robot
 {
 public:
@@ -240,6 +254,29 @@ class Ada final : public aikido::robot::Robot
   /// \return true if all controllers have been successfully switched
   bool stopTrajectoryExecutor();
 
+  /// Toggle controllers and enable/disable cartesian velocity control.
+  /// Required before calling \c moveArmCommandVelocity
+  /// \param[in] useFT whether to use the FT-enabled controller or not.
+  /// \return true if all controllers have been successfully switched
+  bool setVelocityControl(bool enabled, bool useFT = false);
+
+  /// Moves the end effector at a cartesian velocity for a specified
+  /// amount of time.
+  /// \param[in] linear, 3-D velocity in world space, m/s
+  /// \param[in] angular, 3-D velocity about world axes, rad/s
+  /// \param[in] forTime, maximum time to execute velocity 
+  /// \param[in] block, block until completed, cancelled, or aborted
+  /// \return True if the trajectory was completed successfully.
+  std::future<CartVelocityResult> moveArmCommandVelocity(
+    const Eigen::Vector3d& linear,
+    const Eigen::Vector3d& angular = Eigen::Vector3d(0.0, 0.0, 0.0),
+    ros::Duration forTime = ros::Duration(1.0),
+    bool block = false);
+
+  /// Stops any movement created by \c moveArmCommandVelocity
+  /// \return True on successful cancellation
+  bool cancelCommandVelocity();
+
   /// Sets VectorFieldPlanner parameters.
   /// TODO: To be removed with Planner API.
   /// \param[in] vfParameters VectorField Parameters
@@ -336,6 +373,15 @@ class Ada final : public aikido::robot::Robot
       const std::vector<std::string>& startControllers,
       const std::vector<std::string>& stopControllers);
 
+// Short-cuts for action client types
+protected:
+  using Action = pr_control_msgs::SetCartesianVelocityAction;
+  using ActionClient = actionlib::SimpleActionClient<Action>;
+  using GoalState = actionlib::SimpleClientGoalState;
+
+  using Result = pr_control_msgs::SetCartesianVelocityResult;
+  using ResultPtr = pr_control_msgs::SetCartesianVelocityResultConstPtr;
+
 private:
   // Named Configurations are read from a YAML file
   using ConfigurationMap = std::unordered_map<std::string, Eigen::VectorXd>;
@@ -442,6 +488,19 @@ class Ada final : public aikido::robot::Robot
 
   // For trajectory executions.
   std::unique_ptr<aikido::common::ExecutorThread> mThread;
+
+  ////// Cartesian Velocity Variables
+  // Name of cartesian velocity action server
+  const std::string mCartesianVelocityName = "move_until_touch_cartvel_controller";
+  const std::string mCartesianVelocityNoFTName = "cartvel_controller";
+  // Action Client
+  std::shared_ptr<ActionClient> mActionClient;
+  // Transition callback for velocity goals
+  void doneCallback(const GoalState& handle, const ResultPtr& result);
+  // Promise to keep track of current velocity goal
+  std::shared_ptr<std::promise<CartVelocityResult>> mPromise;
+  // Track whether using velocity control
+  bool mVelocityControl;
 };
 
 } // namespace ada

src/Ada.cpp

@@ -117,6 +117,7 @@ Ada::Ada(
   , mSmootherFeasibilityApproxTolerance(1e-3)
   , mWorld(std::move(env))
   , mEndEffectorName(endEffectorName)
+  , mVelocityControl(false)
 {
   simulation = true; // temporarily set simulation to true
 
@@ -459,6 +460,175 @@ bool Ada::stopTrajectoryExecutor()
                                mHandTrajectoryExecutorName});
 }
 
+//=============================================================================
+bool Ada::setVelocityControl(bool enabled, bool useFT) {
+  if(mSimulation) return false;
+  // Do not re-enable or re-disable
+  if(mVelocityControl == enabled) return true;
+
+  bool ret;
+
+  controller_manager_msgs::SwitchController srv;
+  srv.request.strictness
+      = controller_manager_msgs::SwitchControllerRequest::STRICT;
+
+  const std::string controlName = useFT ? mCartesianVelocityName : mCartesianVelocityNoFTName;
+
+  if(enabled) {
+    mActionClient = std::make_shared<ActionClient>(controlName + "/cart_velocity");
+    // Stop joint trajectory control
+    ret = stopTrajectoryExecutor();
+    if (!ret) return false;
+
+    // Switch controllers
+    srv.request.start_controllers = std::vector<std::string>{controlName};
+    srv.request.stop_controllers = std::vector<std::string>();
+    if (!mControllerServiceClient->call(srv) || !srv.response.ok) {
+      return false;
+    }
+
+    // Ensure action client is running
+    ROS_INFO_STREAM("Starting cartesian velocity action client...");
+    if(!mActionClient->waitForServer(ros::Duration(10.0))) {
+      return false;
+    }
+    mVelocityControl = true;
+    return true;
+  } else {
+    // Kill action goals
+    ret = cancelCommandVelocity();
+    if (!ret) return false;
+
+    // Switch controllers
+    srv.request.start_controllers = std::vector<std::string>();
+    srv.request.stop_controllers = std::vector<std::string>{controlName};
+    if (!mControllerServiceClient->call(srv) || !srv.response.ok) {
+      return false;
+    }
+
+    // Start joint trajectory control
+    if(!startTrajectoryExecutor()) {
+      return false;
+    }
+    mVelocityControl = false;
+    return true;
+  }
+}
+
+//=============================================================================
+std::future<CartVelocityResult> Ada::moveArmCommandVelocity(
+    const Eigen::Vector3d& linear,
+    const Eigen::Vector3d& angular,
+    ros::Duration forTime,
+    bool block) {
+  mPromise.reset(new std::promise<CartVelocityResult>());
+  std::future<CartVelocityResult> ret = mPromise->get_future();
+  if (mSimulation || !mVelocityControl) {
+    mPromise->set_value(kCVR_INVALID);
+    return ret;
+  }
+
+  // Construct goal
+  pr_control_msgs::SetCartesianVelocityGoal goal;
+  goal.exec_time = forTime;
+  goal.command.linear.x = linear(0);
+  goal.command.linear.y = linear(1);
+  goal.command.linear.z = linear(2);
+  goal.command.angular.x = angular(0);
+  goal.command.angular.y = angular(1);
+  goal.command.angular.z = angular(2);
+
+  // Send goal
+  using std::placeholders::_1;
+  using std::placeholders::_2;
+  if (mActionClient->getState() != actionlib::SimpleClientGoalState::LOST) {
+    mActionClient->stopTrackingGoal();
+  }
+  mActionClient->sendGoal(goal, 
+    std::bind(&Ada::doneCallback, this, _1, _2));
+
+  // Block until done or time-out
+  if(block) {
+    std::future_status status = ret.wait_for(std::chrono::nanoseconds(forTime.toNSec()));
+    if (status != std::future_status::ready) {
+      // Time-out, write false
+      mPromise->set_value(kCVR_TIMEOUT);
+    }
+  }
+
+  return ret;
+}
+
+void Ada::doneCallback(const GoalState& handle, const ResultPtr& result) {
+  // Only handle current goal
+  if (!mVelocityControl) return;
+
+  if (handle.isDone())
+  {
+    std::stringstream message;
+    CartVelocityResult isSuccessful = kCVR_SUCCESS;
+
+    // Check the status of execution.
+    if (result && result->error_code != Result::SUCCESSFUL)
+    {
+      message << ": ";
+      switch(result->error_code) {
+        case Result::INVALID_CMD:
+          message << "INVALID_CMD";
+          isSuccessful = kCVR_INVALID;
+          break;
+        case Result::FORCE_THRESH:
+          message << "FORCE_THRESH";
+          isSuccessful = kCVR_FORCE;
+          break;
+        case Result::CANCELLED:
+          message << "CANCELLED";
+          isSuccessful = kCVR_CANCELLED;
+          break;
+        default:
+          message << "UNKNOWN";
+          isSuccessful = kCVR_UNKNOWN;
+      }
+
+      if (!result->error_string.empty())
+        message << " (" << result->error_string << ")";
+    }
+    // we can fail without a result for another reason
+    else if (!result) {
+      message << "Velocity Command " << handle.toString();
+
+      const auto terminalMessage = handle.getText();
+      if (!terminalMessage.empty())
+        message << " (" << terminalMessage << ")";
+
+      isSuccessful = kCVR_UNKNOWN;
+    }
+
+    if(isSuccessful != kCVR_SUCCESS) {
+      ROS_WARN_STREAM(message.str());
+    }
+
+    try {
+      mPromise->set_value(isSuccessful);
+    } catch(...) {
+      ROS_WARN_STREAM("Promise already satisfied.");
+    }
+  }
+
+}
+
+//=============================================================================
+bool Ada::cancelCommandVelocity() {
+  if(mSimulation) return false;
+  if(!mActionClient->isServerConnected()) return false;
+  if(!mVelocityControl) return false;
+  if (mActionClient->getState() != actionlib::SimpleClientGoalState::LOST) {
+    mActionClient->stopTrackingGoal();
+  }
+  mActionClient->cancelAllGoals();
+  return true;
+}
+
 //=============================================================================
 void Ada::setVectorFieldPlannerParameters(
     const VectorFieldPlannerParameters& vfParameters)
@@ -576,8 +746,10 @@ bool Ada::switchControllers(
 
   if (mControllerServiceClient->call(srv) && srv.response.ok)
   {
-    mArmTrajectoryExecutorName = startControllers[0];
-    mTrajectoryExecutor = createTrajectoryExecutor();
+    if(startControllers.size() > 0) {
+      mArmTrajectoryExecutorName = startControllers[0];
+      mTrajectoryExecutor = createTrajectoryExecutor();
+    }
     ROS_INFO_STREAM("Controllers switched");
     return true;
   }