Merge pull request #2 from yifan-hou/bimanual

Varies updates for bimanual testing

hardware/ati_netft/src/ati_netft.cpp

@@ -63,7 +63,6 @@ void* ATI_Monitor(void* pParam) {
 ATINetft::ATINetft() {
   _force = RUT::Vector3d::Zero();
   _force_old = RUT::Vector3d::Zero();
-  _WrenchSafety = RUT::Vector6d::Zero();
   _torque = RUT::Vector3d::Zero();
   _torque_old = RUT::Vector3d::Zero();
   _stall_counts = 0;
@@ -126,6 +125,18 @@ int ATINetft::getWrenchSensor(RUT::Vector6d& wrench) {
     }
   }
 
+  // safety
+  for (int i = 0; i < 6; ++i) {
+    if (abs(wrench[i]) > _config.WrenchSafety[i]) {
+      std::cout << "\033[1;31m[ATINetft] Force magnitude is above the safety "
+                   "threshold:\033[0m\n";
+      std::cout << "  feedback:" << wrench.transpose() << std::endl;
+      std::cout << "  safety limit: " << _config.WrenchSafety.transpose()
+                << std::endl;
+    }
+    return -1;
+  }
+
   return 0;
 }
 

workcell/table_top_manip/include/table_top_manip/manip_server.h

@@ -45,6 +45,7 @@ struct ManipServerConfig {
   bool use_perturbation_generator{false};
   CameraSelection camera_selection{CameraSelection::NONE};
   ForceSensingMode force_sensing_mode{ForceSensingMode::NONE};
+  RUT::Matrix6d low_damping{};
 
   bool deserialize(const YAML::Node& node) {
     try {
@@ -64,6 +65,10 @@ struct ManipServerConfig {
           node["camera_selection"].as<std::string>());
       force_sensing_mode = string_to_enum<ForceSensingMode>(
           node["force_sensing_mode"].as<std::string>());
+
+      low_damping = RUT::deserialize_vector<RUT::Vector6d>(node["low_damping"])
+                        .asDiagonal();
+
     } catch (const std::exception& e) {
       std::cerr << "Failed to load the config file: " << e.what() << std::endl;
       return false;
@@ -104,6 +109,7 @@ class ManipServer {
   void join_threads();
   bool is_ready();  // check if all buffers are full
   bool is_running();
+  bool is_bimanual() { return _config.bimanual; }
 
   // getters: get the most recent k data points in the buffer
   const Eigen::MatrixXd get_camera_rgb(int k, int camera_id = 0);
@@ -142,8 +148,6 @@ class ManipServer {
   void stop_saving_data();
   bool is_saving_data();
 
-  bool is_bimanual() { return _config.bimanual; }
-
  private:
   // config
   ManipServerConfig _config;
@@ -177,6 +181,8 @@ class ManipServer {
   // additional configs as local variables
   std::vector<RUT::Matrix6d> _stiffnesses_high{};
   std::vector<RUT::Matrix6d> _stiffnesses_low{};
+  std::vector<RUT::Matrix6d> _dampings_high{};
+  std::vector<RUT::Matrix6d> _dampings_low{};
 
   //  hardware interfaces
   std::vector<std::shared_ptr<CameraInterfaces>> camera_ptrs;

workcell/table_top_manip/src/manip_server.cc

@@ -202,6 +202,8 @@ bool ManipServer::initialize(const std::string& config_path) {
 
     _stiffnesses_high.push_back(admittance_config.compliance6d.stiffness);
     _stiffnesses_low.push_back(RUT::Matrix6d::Zero());
+    _dampings_high.push_back(admittance_config.compliance6d.damping);
+    _dampings_low.push_back(_config.low_damping);
   }
 
   // create the data buffers
@@ -464,6 +466,7 @@ void ManipServer::set_high_level_maintain_position() {
     std::lock_guard<std::mutex> lock(_controller_mtxs[id]);
     // set the robot to have high stiffness, but still compliant
     _controllers[id].setStiffnessMatrix(_stiffnesses_high[id]);
+    _controllers[id].setDampingMatrix(_dampings_high[id]);
   }
 }
 
@@ -475,6 +478,7 @@ void ManipServer::set_high_level_free_jogging() {
     std::lock_guard<std::mutex> lock(_controller_mtxs[id]);
     // set the robot to be compliant
     _controllers[id].setStiffnessMatrix(_stiffnesses_low[id]);
+    _controllers[id].setDampingMatrix(_dampings_low[id]);
   }
 }
 

workcell/table_top_manip/src/manip_server_loops.cc

@@ -333,7 +333,12 @@ void ManipServer::wrench_loop(const RUT::TimePoint& time0, int publish_rate,
         std::lock_guard<std::mutex> lock(_poses_fb_mtxs[id]);
         pose_fb = _poses_fb[id];
       }
-      force_sensor_ptrs[id]->getWrenchNetTool(pose_fb, wrench_fb);
+      if (force_sensor_ptrs[id]->getWrenchNetTool(pose_fb, wrench_fb) < 0) {
+        std::cout << header
+                  << "Wrench is above safety threshold. Ending thread."
+                  << std::endl;
+        break;
+      }
       time_now_ms = timer.toc_ms();
       {
         std::lock_guard<std::mutex> lock(_wrench_buffer_mtxs[id]);

workcell/table_top_manip/src/manip_server_pybind.cc

@@ -17,6 +17,7 @@ PYBIND11_MODULE(manip_server_pybind, m)
         .def("join_threads", &ManipServer::join_threads)
         .def("is_running", &ManipServer::is_running)
         .def("is_ready", &ManipServer::is_ready)
+        .def("is_bimanual", &ManipServer::is_bimanual)
         .def("get_camera_rgb", &ManipServer::get_camera_rgb,
                 py::arg(), py::arg("camera_id") = 0)
         .def("get_wrench", &ManipServer::get_wrench,