Support coinft to ManipServer

FTInterfaces:
* Added interface to get number of sensors
* Changed Vector6d to VectorXd
* Change all getter interface to support multiple sensors

CoinFt,RobotiqFT, ATINetFT:
* Update interfaces to support multiple sensors
* Added size check for wrench arguments
* Moved common internal variables to parent class

CoinFT:
* Added config class and yaml deserialization
* Inherit from FTInterfaces and added common interfaces

ManipServer:
* Updated buffer initialization to support multiple sensors per FT sensor
* Updated wrench loop, mock mode, data logging to support multiple
  sensors

Others
* Update test scripts that assumes a single wrench per sensor

applications/force_control_demo/config/force_control_demo.yaml

@@ -1,5 +1,3 @@
-ft_use_coinft: true
-
 # ur_rtde:
 #   robot_ip: "192.168.2.105"
 #   rtde_frequency: 500.0
@@ -42,8 +40,16 @@ coin_ft:
   port: "/dev/ttyACM0"
   baud_rate: 115200
   calibration_file: "/home/yifanhou/git/hardware_interfaces/hardware/coinft/config/calMat_UFT6.csv"
+  noise_level: 0.0
+  stall_threshold: 100
+  Foffset: [1.88304, -15.861, 11.4758]
+  Toffset: [0.576757, 0.054351, -0.0933367]
+  Gravity: [-0.0512774, 0.0304736, -0.804662]
+  Pcom: [0.000241627, 0.000264568, 0.0835715]
+  WrenchSafety: [40.0, 40.0, 60.0, 3, 3, 3]
   PoseSensorTool: [0.2, 0.005, 0.085, 0.5, -0.5, -0.5,  0.5]
 
+
 admittance_controller:
   dt: 0.002
   log_to_file: false

applications/force_control_demo/src/demo_with_coinft.cc

@@ -28,6 +28,7 @@ T deserialize_vector(const YAML::Node& node) {
 
 int main() {
   URRTDE::URRTDEConfig robot_config;
+  CoinFT::CoinFTConfig coinft_config;
   AdmittanceController::AdmittanceControllerConfig admittance_config;
 
   // open file
@@ -46,36 +47,27 @@ int main() {
   try {
     config = YAML::LoadFile(CONFIG_PATH);
     robot_config.deserialize(config["ur_rtde"]);
+    coinft_config.deserialize(config["coin_ft"]);
     deserialize(config["admittance_controller"], admittance_config);
-    ft_use_coinft = config["ft_use_coinft"].as<bool>();
-
-    coinft_port = config["coin_ft"]["port"].as<std::string>();
-    coinft_baud_rate = config["coin_ft"]["baud_rate"].as<unsigned int>();
-    coinft_calibration_file =
-        config["coin_ft"]["calibration_file"].as<std::string>();
-    coinft_PoseSensorTool =
-        config["coin_ft"]["PoseSensorTool"].as<std::vector<double>>();
-    RUT::Vector7d pose_sensor_tool;
-    pose_sensor_tool = RUT::Vector7d::Map(coinft_PoseSensorTool.data(), 7);
-    adj_sensor_tool = RUT::SE32Adj(RUT::pose2SE3(pose_sensor_tool));
+
   } catch (const std::exception& e) {
     std::cerr << "Failed to load the config file: " << e.what() << std::endl;
     return -1;
   }
 
   URRTDE robot;
+  CoinFT sensor;
   AdmittanceController controller;
   RUT::Timer timer;
   RUT::TimePoint time0 = timer.tic();
   RUT::Vector7d pose, pose_ref, pose_cmd;
-  RUT::Vector6d wrench_S, wrench_T, wrench_WTr;
+  RUT::VectorXd wrench, wrench_WTr;
   wrench_WTr.setZero();
 
   robot.init(time0, robot_config);
   robot.getCartesian(pose);
 
-  CoinFT sensor(coinft_port, coinft_baud_rate, coinft_calibration_file);
-  sensor.startStreaming();
+  sensor.init(time0, coinft_config);
 
   std::cout << "Starting in 2 seconds ..." << std::endl;
   sleep(2.0);
@@ -97,7 +89,11 @@ int main() {
   controller.setForceControlledAxis(Tr, n_af);
 
   pose_ref = pose;
-  wrench_T.setZero();
+
+  while (!sensor.is_data_ready()) {
+    std::cout << "Waiting for data..." << std::endl;
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
 
   timer.tic();
 
@@ -107,27 +103,9 @@ int main() {
     robot.getCartesian(pose);
 
     // read wrench
-    // robot.getWrenchTool(wrench);
-    std::vector<double> ftData = sensor.getLatestData();
-
-    if (!ftData.empty()) {
-      std::cout << "[" << timer.toc_ms() << " ms] "
-                << "Force/Torque Data: ";
-      for (const auto& value : ftData) {
-        std::cout << value << " ";
-      }
-      std::cout << std::endl;
-    } else {
-      std::cout << "No data available yet." << std::endl;
-      continue;
-    }
-    for (size_t i = 0; i < 6; i++) {
-      wrench_S[i] = ftData[i];
-    }
-    // Transform sensor wrench to robot base frame
-    wrench_T = adj_sensor_tool.transpose() * wrench_S;
+    sensor.getWrenchTool(wrench);
 
-    controller.setRobotStatus(pose, wrench_T);
+    controller.setRobotStatus(pose, wrench);
 
     // Update robot reference
     controller.setRobotReference(pose_ref, wrench_WTr);
@@ -141,8 +119,8 @@ int main() {
     }
 
     double dt = timer.toc_ms();
-    printf("t = %f, wrench: %f %f %f %f %f %f\n", dt, wrench_T[0], wrench_T[1],
-           wrench_T[2], wrench_T[3], wrench_T[4], wrench_T[5]);
+    printf("t = %f, wrench: %f %f %f %f %f %f\n", dt, wrench[0], wrench[1],
+           wrench[2], wrench[3], wrench[4], wrench[5]);
 
     if (dt > 3000)
       break;

applications/ft_calibration/src/main.cc

@@ -2,6 +2,7 @@
 #include <memory>
 
 #include <yaml-cpp/yaml.h>
+#include <Eigen/Dense>
 
 #include <RobotUtilities/spatial_utilities.h>
 #include <RobotUtilities/timer_linux.h>
@@ -138,7 +139,8 @@ int main() {
   RUT::Timer timer;
   RUT::TimePoint time0 = timer.tic();
   RUT::Vector7d pose, pose_ref, pose_cmd;
-  RUT::Vector6d wrench, wrench_WTr;
+  RUT::Vector6d wrench_6d, wrench_WTr;
+  RUT::VectorXd wrench;
 
   robot.init(time0, robot_config);
   robot.getCartesian(pose);
@@ -187,8 +189,8 @@ int main() {
     RUT::TimePoint t_start = robot.rtde_init_period();
     // Update robot status
     robot.getCartesian(pose);
-    robot.getWrenchTool(wrench);
-    controller.setRobotStatus(pose, wrench);
+    robot.getWrenchTool(wrench_6d);
+    controller.setRobotStatus(pose, wrench_6d);
 
     // Update robot reference
     controller.setRobotReference(pose_ref, wrench_WTr);

hardware/ati_netft/include/ati_netft/ati_netft.h

@@ -86,15 +86,15 @@ class ATINetft : public FTInterfaces {
    * @return  0: no error.
    *   1: still waiting for new data. 2: dead stream.
    */
-  int getWrenchSensor(RUT::Vector6d& wrench) override;
+  int getWrenchSensor(RUT::VectorXd& wrench, int num_of_sensors = 1) override;
   /**
    * Get the wrench in tool frame.
    *
    * @param      wrench_T  The wrench in tool frame
    *
    * @return     0: no error. 1: still waiting for new data. 2: dead stream.
    */
-  int getWrenchTool(RUT::Vector6d& wrench_T) override;
+  int getWrenchTool(RUT::VectorXd& wrench_T, int num_of_sensors = 1) override;
   /**
    * Get the tool wrench after tool weight compensation.
    *
@@ -104,26 +104,17 @@ class ATINetft : public FTInterfaces {
    * @return     0: no error. 1: still waiting for new data. 2: dead stream.
    *             3: force is too big.
    */
-  int getWrenchNetTool(const RUT::Vector7d& pose,
-                       RUT::Vector6d& wrench_net_T) override;
+  int getWrenchNetTool(const RUT::Vector7d& pose, RUT::VectorXd& wrench_net_T,
+                       int num_of_sensors = 1) override;
 
-  // pre-allocated internal variables
-  RUT::Vector3d _force, _force_old;
-  RUT::Vector3d _torque, _torque_old;
-  RUT::Vector6d _wrench_sensor_temp, _wrench_tool_temp;
-  RUT::Matrix3d _R_WT;
-  RUT::Vector3d _GinF, _GinT;
+  int getNumSensors() override { return 1; }
 
   RUT::TimePoint _time0;  ///< high resolution timer.
   std::ofstream _file;
   ATINetftConfig _config;
   // netft
   std::shared_ptr<netft_rdt_driver::NetFTRDTDriver> _netft;
 
-  // monitor pausing of the data stream.
-  // if the data is the same in 50 frames, the stream is considered dead.
-  int _stall_counts;
-
  private:
   // thread
   pthread_t _thread;

hardware/ati_netft/src/ati_netft.cpp

@@ -105,7 +105,9 @@ bool ATINetft::init(RUT::TimePoint time0, const ATINetftConfig& config) {
   return true;
 }
 
-int ATINetft::getWrenchSensor(RUT::Vector6d& wrench) {
+int ATINetft::getWrenchSensor(RUT::VectorXd& wrench, int num_of_sensors) {
+  assert(num_of_sensors == 1);
+  assert(wrench.size() == 6);
   wrench.head(3) = _force;
   wrench.tail(3) = _torque;
 
@@ -140,15 +142,16 @@ int ATINetft::getWrenchSensor(RUT::Vector6d& wrench) {
   return 0;
 }
 
-int ATINetft::getWrenchTool(RUT::Vector6d& wrench_T) {
-  int flag = getWrenchSensor(_wrench_sensor_temp);
+int ATINetft::getWrenchTool(RUT::VectorXd& wrench_T, int num_of_sensors) {
+  int flag = this->getWrenchSensor(_wrench_sensor_temp);
   wrench_T = _adj_sensor_tool.transpose() * _wrench_sensor_temp;
   return flag;
 }
 
 int ATINetft::getWrenchNetTool(const RUT::Vector7d& pose,
-                               RUT::Vector6d& wrench_net_T) {
-  int flag = getWrenchTool(_wrench_tool_temp);
+                               RUT::VectorXd& wrench_net_T,
+                               int num_of_sensors) {
+  int flag = this->getWrenchTool(_wrench_tool_temp);
 
   // compensate for the weight of object
   _R_WT = RUT::quat2SO3(pose[3], pose[4], pose[5], pose[6]);

hardware/coinft/CMakeLists.txt

@@ -9,10 +9,10 @@ include_directories(include)
 
 # Add the source files
 add_library(COIN_FT src/coin_ft.cpp)
-target_link_libraries(COIN_FT ${Boost_LIBRARIES} Eigen3::Eigen)
+target_link_libraries(COIN_FT ${Boost_LIBRARIES} ${RUT} ${YAMLLib} Eigen3::Eigen)
 
 add_executable(coinft_test src/coinft_test.cpp)
-target_link_libraries(coinft_test COIN_FT)
+target_link_libraries(coinft_test COIN_FT ${RUT})
 
 # Install the library
 install(DIRECTORY include/

hardware/coinft/include/coinft/coin_ft.h

@@ -11,7 +11,9 @@
 
 #ifndef COINFT_H
 #define COINFT_H
-
+#include <RobotUtilities/spatial_utilities.h>
+#include <RobotUtilities/timer_linux.h>
+#include <yaml-cpp/yaml.h>
 #include <Eigen/Dense>
 #include <atomic>  // For atomic flags
 #include <boost/asio.hpp>
@@ -23,40 +25,118 @@
 #include <thread>
 #include <vector>
 
-class CoinFT {
+#include "hardware_interfaces/ft_interfaces.h"
+
+typedef std::chrono::high_resolution_clock Clock;
+
+class CoinFT : public FTInterfaces {
  public:
-  CoinFT(const std::string& port, unsigned int baud_rate,
-         const std::string& calibration_file);
+  struct CoinFTConfig {
+    std::string port{"/dev/ttyACM0"};
+    unsigned int baud_rate{115200};
+    std::string calibration_file{"netft"};
+    int num_sensors{1};
+    // 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};
+    int stall_threshold{50};
+    RUT::Vector3d Foffset{};
+    RUT::Vector3d Toffset{};
+    RUT::Vector3d Gravity{};
+    RUT::Vector3d Pcom{};
+    RUT::Vector6d WrenchSafety{};
+    RUT::Vector7d PoseSensorTool{};
+
+    bool deserialize(const YAML::Node& node) {
+      try {
+        port = node["port"].as<std::string>();
+        baud_rate = node["baud_rate"].as<unsigned int>();
+        calibration_file = node["calibration_file"].as<std::string>();
+        num_sensors = node["num_sensors"].as<int>();
+        noise_level = node["noise_level"].as<double>();
+        stall_threshold = node["stall_threshold"].as<int>();
+        Foffset = RUT::deserialize_vector<RUT::Vector3d>(node["Foffset"]);
+        Toffset = RUT::deserialize_vector<RUT::Vector3d>(node["Toffset"]);
+        Gravity = RUT::deserialize_vector<RUT::Vector3d>(node["Gravity"]);
+        Pcom = RUT::deserialize_vector<RUT::Vector3d>(node["Pcom"]);
+        WrenchSafety =
+            RUT::deserialize_vector<RUT::Vector6d>(node["WrenchSafety"]);
+        PoseSensorTool =
+            RUT::deserialize_vector<RUT::Vector7d>(node["PoseSensorTool"]);
+      } catch (const std::exception& e) {
+        std::cerr << "Failed to load the config file: " << e.what()
+                  << std::endl;
+        return false;
+      }
+      return true;
+    }
+  };
+
+  CoinFT();
   ~CoinFT();
+  bool init(RUT::TimePoint time0, const CoinFTConfig& coinft_config);
+  /**
+   * Get the sensor reading.
+   *
+   * @param  wrench  The wrench
+   *
+   * @return  0: no error.
+   *   1: still waiting for new data. 2: dead stream.
+   */
+  int getWrenchSensor(RUT::VectorXd& wrench, int num_of_sensors = 1) override;
+  /**
+   * Get the wrench in tool frame.
+   *
+   * @param      wrench_T  The wrench in tool frame
+   *
+   * @return     0: no error. 1: still waiting for new data. 2: dead stream.
+   */
+  int getWrenchTool(RUT::VectorXd& wrench_T, int num_of_sensors = 1) override;
+  /**
+   * Get the tool wrench after tool weight compensation.
+   *
+   * @param[in]  pose          The Cartesian pose of the robot tool
+   * @param      wrench_net_T  The net wrench in tool frame
+   *
+   * @return     0: no error. 1: still waiting for new data. 2: dead stream.
+   *             3: force is too big.
+   */
+  int getWrenchNetTool(const RUT::Vector7d& pose, RUT::VectorXd& wrench_net_T,
+                       int num_of_sensors = 1) override;
+
+  int getNumSensors() override { return _config.num_sensors; }
 
   void startStreaming();
   void stopStreaming();
   void closePort();
   void tare();
 
-  std::vector<double> getLatestData();
-  std::atomic<uint64_t> reading_counter;
-
  private:
+  RUT::TimePoint _time0;
+  CoinFTConfig _config;
+
+  // CoinFT internal variables
   enum Commands {
     IDLE = 0x69,    // 'i'
     QUERY = 0x71,   // 'q'
     STREAM = 0x73,  // 's'
   };
+  std::atomic<uint64_t> reading_counter;
 
   static const uint8_t STX = 0x02;
   static const uint8_t ETX = 0x03;
 
+  std::vector<double> getLatestData();
   void initializeSensor();
   void sendChar(uint8_t cmd);
   std::vector<uint8_t> readData(size_t length);
   bool readRawData(std::vector<uint16_t>& rawData);
   void dataAcquisitionLoop();  // The function run by the background thread
 
-  boost::asio::io_service io;
-  boost::asio::serial_port serial;
+  std::shared_ptr<boost::asio::io_service> io_ptr;
+  std::shared_ptr<boost::asio::serial_port> serial_ptr;
   int packet_size;
-  int num_sensors;
+  int num_sensor_units;  // CoinFT internal parameter
 
   std::thread data_thread;
   std::mutex data_mutex;