Add Cartesian Velocity Controller

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}"

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

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;
   }
 

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 {
 
 //=============================================================================

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>