Added WSGGripper and CoinFT to ManipServer

CoinFT
* Added COINFT as a new ForceSensingMode in types.h
* Update config files
* Added num_sensors and publish rate to CoinFT config
* Change CoinFT lib to shared library

WSGGripper
* Fixed CMakeLists.txt bug for WSGGripper
* Fixed eigen conversion bugs in WSGGripper by replacing Vector7d to
  VectorXd, fixed a bunch of typos
* Fixed float to double conversion error by using static_cast

ManipServer
* Introduced state/buffer/timestamps/thread/mutex/flow control/getter/setters
  members for EoAT
* Added option to select CoinFT as wrench sensor
* Added a loop function for EOAT, handles scheduled waypoints with
  interpolation
* Added schedule_eoat_waypoints function
* Added data logging function for EOAT

applications/force_control_demo/config/force_control_demo.yaml

@@ -40,6 +40,8 @@ coin_ft:
   port: "/dev/ttyACM0"
   baud_rate: 115200
   calibration_file: "/home/yifanhou/git/hardware_interfaces/hardware/coinft/config/calMat_UFT6.csv"
+  num_sensors: 1
+  publish_rate: 360
   noise_level: 0.0
   stall_threshold: 100
   Foffset: [1.88304, -15.861, 11.4758]

hardware/coinft/CMakeLists.txt

@@ -8,7 +8,7 @@ include_directories(${Boost_INCLUDE_DIRS})
 include_directories(include)
 
 # Add the source files
-add_library(COIN_FT src/coin_ft.cpp)
+add_library(COIN_FT SHARED src/coin_ft.cpp)
 target_link_libraries(COIN_FT ${Boost_LIBRARIES} ${RUT} ${YAMLLib} Eigen3::Eigen)
 
 add_executable(coinft_test src/coinft_test.cpp)

hardware/coinft/include/coinft/coin_ft.h

@@ -36,6 +36,7 @@ class CoinFT : public FTInterfaces {
     unsigned int baud_rate{115200};
     std::string calibration_file{"netft"};
     int num_sensors{1};
+    int publish_rate{360};
     // If the force change is smaller than noise_level for more than stall_threshold frames,
     // the stream is considered dead.
     double noise_level{0.0};
@@ -53,6 +54,7 @@ class CoinFT : public FTInterfaces {
         baud_rate = node["baud_rate"].as<unsigned int>();
         calibration_file = node["calibration_file"].as<std::string>();
         num_sensors = node["num_sensors"].as<int>();
+        publish_rate = node["publish_rate"].as<int>();
         noise_level = node["noise_level"].as<double>();
         stall_threshold = node["stall_threshold"].as<int>();
         Foffset = RUT::deserialize_vector<RUT::Vector3d>(node["Foffset"]);

hardware/wsg_gripper/CMakeLists.txt

@@ -4,6 +4,7 @@ include_directories(
 )
 
 add_library(WSG_GRIPPER SHARED
+  src/wsg_gripper.cpp
   src/WSG50Controller.cpp
   src/WSG50Subject.cpp
   src/WSG50Communicator.cpp

hardware/wsg_gripper/include/wsg_gripper/wsg_gripper.h

@@ -46,6 +46,7 @@ class WSGGripper : public JSInterfaces {
 
   WSGGripper();
   ~WSGGripper();
+
   /**
    * Initialize the wsg controller
    *
@@ -55,12 +56,12 @@ class WSGGripper : public JSInterfaces {
    * @return     True if success.
    */
   bool init(RUT::TimePoint time0, const WSGGripperConfig& config);
-  bool checkJointTarget(RUT::Vector7d& pose_xyzq_set);
+  bool checkJointTarget(RUT::VectorXd& pose_xyzq_set);
 
   bool getJoints(RUT::VectorXd& joints) override;
   bool setJoints(const RUT::VectorXd& joints) override;
-  bool setJointsPosForceForce(const RUT::VectorXd& joints,
-                              const RUT::VectorXd& forces) override;
+  bool setJointsPosForce(const RUT::VectorXd& joints,
+                         const RUT::VectorXd& forces) override;
 
  private:
   RUT::TimePoint _time0;

hardware/wsg_gripper/src/wsg_gripper.cpp

@@ -5,15 +5,18 @@
 
 #include <RobotUtilities/spatial_utilities.h>
 
-bool WSGGripper::initialize(
-    RUT::TimePoint time0,
-    const WSGGripper::WSGGripperConfig& wsg_gripper_config) {
+WSGGripper::WSGGripper() {}
+
+WSGGripper::~WSGGripper() {}
+
+bool WSGGripper::init(RUT::TimePoint time0,
+                      const WSGGripper::WSGGripperConfig& wsg_gripper_config) {
   _time0 = time0;
   _config = wsg_gripper_config;
 
   /* Establish connection with WSG gripper */
-  std::cout << "[WSGGripper] Connecting to gripper at " << config.robot_ip
-            << ", port " << config.port << std::endl;
+  std::cout << "[WSGGripper] Connecting to gripper at " << _config.robot_ip
+            << ", port " << _config.port << std::endl;
   _wsg_ptr = std::make_shared<WSG50Controller>(_config.robot_ip, _config.port);
 
   while (!_wsg_ptr->ready()) {
@@ -28,49 +31,49 @@ bool WSGGripper::initialize(
 }
 
 bool WSGGripper::checkJointTarget(RUT::VectorXd& joints_set) {
-  if (config.js_interface_config.incre_safety_mode !=
+  if (_config.js_interface_config.incre_safety_mode !=
       RobotSafetyMode::SAFETY_MODE_NONE) {
     bool incre_safe = incre_safety_check(joints_set, _joints_set_prev,
-                                         config.js_interface_config.max_incre);
+                                         _config.js_interface_config.max_incre);
     if (!incre_safe) {
       std::cerr << "\033[1;33m[WSGGripper][checkJointTarget] Incremental "
                    "safety check failed.\033[0m\n";
       std::cerr << "set joint: " << joints_set.transpose()
                 << "\nprev set joint: " << _joints_set_prev.transpose()
-                << ", max_incre: " << config.js_interface_config.max_incre
+                << ", max_incre: " << _config.js_interface_config.max_incre
                 << std::endl;
-      if (config.js_interface_config.incre_safety_mode ==
+      if (_config.js_interface_config.incre_safety_mode ==
           RobotSafetyMode::SAFETY_MODE_STOP) {
         std::cerr << "[WSGGripper][checkJointTarget] Returning false."
                   << std::endl;
         return false;
-      } else if (config.js_interface_config.incre_safety_mode ==
+      } else if (_config.js_interface_config.incre_safety_mode ==
                  RobotSafetyMode::SAFETY_MODE_TRUNCATE) {
         // clip the joint set around the previous joint set
         joints_set = _joints_set_prev +
                      (joints_set - _joints_set_prev)
-                         .cwiseMin(config.js_interface_config.max_incre)
-                         .cwiseMax(-config.js_interface_config.max_incre);
+                         .cwiseMin(_config.js_interface_config.max_incre)
+                         .cwiseMax(-_config.js_interface_config.max_incre);
         return false;
       }
     }
   }
 
   bool zone_safe = range_safety_check(
-      joints_set, config.js_interface_config.safe_zone, _joints_set_truncated);
+      joints_set, _config.js_interface_config.safe_zone, _joints_set_truncated);
   if (!zone_safe) {
-    if (config.js_interface_config.range_safety_mode ==
+    if (_config.js_interface_config.range_safety_mode ==
         RobotSafetyMode::SAFETY_MODE_STOP) {
       std::cerr
           << "\033[1;33m[WSGGripper][checkJointTarget] Range safety check "
              "failed.\033[0m\n";
       std::cerr << "[WSGGripper][checkJointTarget] target joints: "
                 << joints_set.transpose() << std::endl;
       std::cerr << "[WSGGripper][checkJointTarget] safe range: "
-                << config.js_interface_config.safe_zone.transpose()
+                << _config.js_interface_config.safe_zone.transpose()
                 << std::endl;
       return false;
-    } else if (config.js_interface_config.range_safety_mode ==
+    } else if (_config.js_interface_config.range_safety_mode ==
                RobotSafetyMode::SAFETY_MODE_TRUNCATE) {
       std::cerr << "[WSGGripper][checkJointTarget] Zone safety check failed. "
                    "Using truncated pose."
@@ -97,7 +100,7 @@ bool WSGGripper::setJoints(const RUT::VectorXd& joints) {
   }
   _joints_set_prev = _joints_set_processed;
 
-  while (!wsgController.ready()) {
+  while (!_wsg_ptr->ready()) {
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
   }
   _wsg_ptr->prePositionFingers(false, _joints_set_processed[0]);
@@ -113,18 +116,16 @@ bool WSGGripper::setJointsPosForce(const RUT::VectorXd& joints,
   }
   _joints_set_prev = _joints_set_processed;
 
-  while (!wsgController.ready()) {
+  while (!_wsg_ptr->ready()) {
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
   }
 
-  _wsg_ptr->setVelResolvedControl(_joints_set_processed[0], forces[0],
-                                  _config.velResControl_stiffness,
-                                  _config.velResControl_damping);
+  _wsg_ptr->setVelResolvedControl(
+      static_cast<float>(_joints_set_processed[0]),
+      static_cast<float>(forces[0]),
+      static_cast<float>(_config.velResControl_stiffness),
+      static_cast<float>(_config.velResControl_damping));
   // _wsg_ptr->setPDControl(_joints_set_processed[0], _config.PDControl_kp,
   //                        _config.PDControl_kd, forces[0]);
   return true;
 }
-
-WSGGripper::WSGGripper() : m_impl{std::make_unique<Implementation>()} {}
-
-WSGGripper::~WSGGripper() {}

include/hardware_interfaces/types.h

@@ -22,7 +22,13 @@ enum class RobotOperationMode {
   OPERATION_MODE_JOINT
 };
 
-enum class ForceSensingMode { NONE, FORCE_MODE_ATI, FORCE_MODE_ROBOTIQ };
+enum class ForceSensingMode {
+  NONE,
+  FORCE_MODE_ATI,
+  FORCE_MODE_ROBOTIQ,
+  FORCE_MODE_COINFT
+};
+
 enum class CameraSelection { NONE, GOPRO, REALSENSE };
 
 enum class RandomType {

src/types.cpp

@@ -19,7 +19,8 @@ const std::vector<RobotOperationMode>& all_operation_modes() {
 const std::vector<ForceSensingMode>& all_force_sensing_modes() {
   static const std::vector<ForceSensingMode> modes = {
       ForceSensingMode::NONE, ForceSensingMode::FORCE_MODE_ATI,
-      ForceSensingMode::FORCE_MODE_ROBOTIQ};
+      ForceSensingMode::FORCE_MODE_ROBOTIQ,
+      ForceSensingMode::FORCE_MODE_COINFT};
   return modes;
 }
 
@@ -76,6 +77,8 @@ const char* to_string(const ForceSensingMode e) {
       return "FORCE_MODE_ATI";
     case ForceSensingMode::FORCE_MODE_ROBOTIQ:
       return "FORCE_MODE_ROBOTIQ";
+    case ForceSensingMode::FORCE_MODE_COINFT:
+      return "FORCE_MODE_COINFT";
     default:
       return "INVALID_FORCE_SENSING_MODE";
   }

workcell/table_top_manip/CMakeLists.txt

@@ -30,6 +30,8 @@ target_link_libraries(ManipServer
   ROBOTIQ_FT_HW
   RQSensorLinux
   UR_RTDE
+  WSG_GRIPPER
+  COIN_FT
   HI_COMMON
   ${RUT}
   ${FORCE_CONTROLLERS}

workcell/table_top_manip/include/table_top_manip/manip_server.h

@@ -17,25 +17,30 @@
 
 #include <force_control/admittance_controller.h>
 #include <force_control/config_deserialize.h>
+#include <hardware_interfaces/js_interfaces.h>
 #include <hardware_interfaces/robot_interfaces.h>
 #include <hardware_interfaces/types.h>
 // hardware used in this app
 #include <ati_netft/ati_netft.h>
+#include <coinft/coin_ft.h>
 #include <gopro/gopro.h>
 #include <realsense/realsense.h>
 #include <robotiq_ft_modbus/robotiq_ft_modbus.h>
 #include <ur_rtde/ur_rtde.h>
+#include <wsg_gripper/wsg_gripper.h>
 
 #include <RobotUtilities/data_buffer.h>
 
 struct ManipServerConfig {
   std::string data_folder{""};
   bool run_robot_thread{false};
+  bool run_eoat_thread{false};
   bool run_wrench_thread{false};
   bool run_rgb_thread{false};
   bool plot_rgb{false};
   int rgb_buffer_size{5};
   int pose_buffer_size{100};
+  int eoat_buffer_size{100};
   int wrench_buffer_size{100};
   bool mock_hardware{false};
   bool bimanual{false};
@@ -48,11 +53,13 @@ struct ManipServerConfig {
     try {
       data_folder = node["data_folder"].as<std::string>();
       run_robot_thread = node["run_robot_thread"].as<bool>();
+      run_eoat_thread = node["run_eoat_thread"].as<bool>();
       run_wrench_thread = node["run_wrench_thread"].as<bool>();
       run_rgb_thread = node["run_rgb_thread"].as<bool>();
       plot_rgb = node["plot_rgb"].as<bool>();
       rgb_buffer_size = node["rgb_buffer_size"].as<int>();
       pose_buffer_size = node["pose_buffer_size"].as<int>();
+      eoat_buffer_size = node["eoat_buffer_size"].as<int>();
       wrench_buffer_size = node["wrench_buffer_size"].as<int>();
       mock_hardware = node["mock_hardware"].as<bool>();
       bimanual = node["bimanual"].as<bool>();
@@ -111,13 +118,15 @@ class ManipServer {
   const Eigen::MatrixXd get_camera_rgb(int k, int camera_id = 0);
   const Eigen::MatrixXd get_wrench(int k, int sensor_id = 0);
   const Eigen::MatrixXd get_pose(int k, int robot_id = 0);
+  const Eigen::MatrixXd get_eoat(int k, int robot_id = 0);
 
   // the following functions return the timestamps of
   //  the most recent getter call of the corresponding feedback
   //  So size is already know
   const Eigen::VectorXd get_camera_rgb_timestamps_ms(int id = 0);
   const Eigen::VectorXd get_wrench_timestamps_ms(int id = 0);
   const Eigen::VectorXd get_pose_timestamps_ms(int id = 0);
+  const Eigen::VectorXd get_eoat_timestamps_ms(int id = 0);
 
   double get_timestamp_now_ms();  // access the current hardware time
 
@@ -133,6 +142,9 @@ class ManipServer {
   void schedule_waypoints(const Eigen::MatrixXd& waypoints,
                           const Eigen::VectorXd& timepoints_ms,
                           int robot_id = 0);
+  void schedule_eoat_waypoints(const Eigen::MatrixXd& waypoints,
+                               const Eigen::VectorXd& timepoints_ms,
+                               int robot_id = 0);
   void schedule_stiffness(const Eigen::MatrixXd& stiffness,
                           const Eigen::VectorXd& timepoints_ms,
                           int robot_id = 0);
@@ -154,21 +166,27 @@ class ManipServer {
   // data buffer
   std::vector<RUT::DataBuffer<Eigen::MatrixXd>> _camera_rgb_buffers;
   std::vector<RUT::DataBuffer<Eigen::VectorXd>> _pose_buffers;
+  std::vector<RUT::DataBuffer<Eigen::VectorXd>> _eoat_buffers;
   std::vector<RUT::DataBuffer<Eigen::VectorXd>> _wrench_buffers;
   // action buffer
+  std::vector<RUT::DataBuffer<Eigen::VectorXd>> _eoat_waypoints_buffers;
   std::vector<RUT::DataBuffer<Eigen::VectorXd>> _waypoints_buffers;
   std::vector<RUT::DataBuffer<Eigen::MatrixXd>> _stiffness_buffers;
 
   std::vector<RUT::DataBuffer<double>> _camera_rgb_timestamp_ms_buffers;
   std::vector<RUT::DataBuffer<double>> _pose_timestamp_ms_buffers;
+  std::vector<RUT::DataBuffer<double>> _eoat_timestamp_ms_buffers;
   std::vector<RUT::DataBuffer<double>> _wrench_timestamp_ms_buffers;
   std::vector<RUT::DataBuffer<double>> _waypoints_timestamp_ms_buffers;
+  std::vector<RUT::DataBuffer<double>> _eoat_waypoints_timestamp_ms_buffers;
   std::vector<RUT::DataBuffer<double>> _stiffness_timestamp_ms_buffers;
 
   std::deque<std::mutex> _camera_rgb_buffer_mtxs;
   std::deque<std::mutex> _pose_buffer_mtxs;
+  std::deque<std::mutex> _eoat_buffer_mtxs;
   std::deque<std::mutex> _wrench_buffer_mtxs;
   std::deque<std::mutex> _waypoints_buffer_mtxs;
+  std::deque<std::mutex> _eoat_waypoints_buffer_mtxs;
   std::deque<std::mutex> _stiffness_buffer_mtxs;
 
   // timing
@@ -184,35 +202,41 @@ class ManipServer {
   std::vector<std::shared_ptr<CameraInterfaces>> camera_ptrs;
   std::vector<std::shared_ptr<FTInterfaces>> force_sensor_ptrs;
   std::vector<std::shared_ptr<RobotInterfaces>> robot_ptrs;
+  std::vector<std::shared_ptr<JSInterfaces>> eoat_ptrs;
   // controllers
   std::vector<AdmittanceController> _controllers;
   std::deque<std::mutex> _controller_mtxs;
 
   // threads
   std::vector<std::thread> _robot_threads;
+  std::vector<std::thread> _eoat_threads;
   std::vector<std::thread> _wrench_threads;
   std::vector<std::thread> _rgb_threads;
   std::thread _rgb_plot_thread;
 
   // control variables to control the threads
   std::vector<std::string> _ctrl_rgb_folders;
   std::vector<std::ofstream> _ctrl_robot_data_streams;
+  std::vector<std::ofstream> _ctrl_eoat_data_streams;
   std::vector<std::ofstream> _ctrl_wrench_data_streams;
   bool _ctrl_flag_running = false;  // flag to terminate the program
   bool _ctrl_flag_saving = false;   // flag for ongoing data collection
   std::mutex _ctrl_mtx;
 
   // state variable indicating the status of the threads
   std::vector<bool> _states_robot_thread_ready{};
+  std::vector<bool> _states_eoat_thread_ready{};
   std::vector<bool> _states_rgb_thread_ready{};
   std::vector<bool> _states_wrench_thread_ready{};
   bool _state_plot_thread_ready{false};
 
   std::vector<bool> _states_robot_thread_saving{};
+  std::vector<bool> _states_eoat_thread_saving{};
   std::vector<bool> _states_rgb_thread_saving{};
   std::vector<bool> _states_wrench_thread_saving{};
 
   std::vector<int> _states_robot_seq_id{};
+  std::vector<int> _states_eoat_seq_id{};
   std::vector<int> _states_rgb_seq_id{};
   std::vector<int> _states_wrench_seq_id{};
 
@@ -227,9 +251,11 @@ class ManipServer {
   // temp variables storing timestamps of data just being fetched
   std::vector<Eigen::VectorXd> _camera_rgb_timestamps_ms;
   std::vector<Eigen::VectorXd> _pose_timestamps_ms;
+  std::vector<Eigen::VectorXd> _eoat_timestamps_ms;
   std::vector<Eigen::VectorXd> _wrench_timestamps_ms;
 
   void robot_loop(const RUT::TimePoint& time0, int robot_id);
+  void eoat_loop(const RUT::TimePoint& time0, int robot_id);
   void rgb_loop(const RUT::TimePoint& time0, int camera_id);
   void wrench_loop(const RUT::TimePoint& time0, int publish_rate,
                    int sensor_id);