Merge pull request #1 from yifan-hou/bimanual

Bimanual Support

hardware/ur_rtde/src/ur_rtde.cpp

@@ -169,7 +169,7 @@ bool URRTDE::Implementation::checkCartesianTarget(
       std::cerr << "\033[1;33m[URRTDE][checkCartesianTarget] Incremental "
                    "safety check failed.\033[0m\n";
       std::cerr << "set pose: " << pose_xyzq_set.transpose()
-                << ", prev pose: " << pose_xyzq_set_prev.transpose()
+                << "\nprev pose: " << pose_xyzq_set_prev.transpose()
                 << ", max_incre_m: "
                 << config.robot_interface_config.max_incre_m
                 << ", max_incre_rad: "

workcell/table_top_manip/include/table_top_manip/manip_server.h

@@ -2,6 +2,7 @@
 
 #include <unistd.h>
 #include <csignal>
+#include <deque>
 #include <iostream>
 #include <memory>
 #include <mutex>
@@ -32,25 +33,31 @@
 
 struct ManipServerConfig {
   std::string data_folder{""};
+  bool run_robot_thread{false};
+  bool run_wrench_thread{false};
+  bool run_rgb_thread{false};
   bool plot_rgb{false};
-  bool run_low_dim_thread{false};
   int rgb_buffer_size{5};
   int pose_buffer_size{100};
   int wrench_buffer_size{100};
   bool mock_hardware{false};
+  bool bimanual{false};
   bool use_perturbation_generator{false};
   CameraSelection camera_selection{CameraSelection::NONE};
   ForceSensingMode force_sensing_mode{ForceSensingMode::NONE};
 
   bool deserialize(const YAML::Node& node) {
     try {
       data_folder = node["data_folder"].as<std::string>();
+      run_robot_thread = node["run_robot_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>();
-      run_low_dim_thread = node["run_low_dim_thread"].as<bool>();
       rgb_buffer_size = node["rgb_buffer_size"].as<int>();
       pose_buffer_size = node["pose_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>();
       use_perturbation_generator =
           node["use_perturbation_generator"].as<bool>();
       camera_selection = string_to_enum<CameraSelection>(
@@ -99,31 +106,34 @@ class ManipServer {
   bool is_running();
 
   // getters: get the most recent k data points in the buffer
-  const Eigen::MatrixXd get_camera_rgb(int k);
-  const Eigen::MatrixXd get_wrench(int k);
-  const Eigen::MatrixXd get_pose(int k);
+  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);
 
   // 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();
-  const Eigen::VectorXd get_wrench_timestamps_ms();
-  const Eigen::VectorXd get_pose_timestamps_ms();
+  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);
 
   double get_timestamp_now_ms();  // access the current hardware time
 
   void set_high_level_maintain_position();
   void set_high_level_free_jogging();
 
   void set_target_pose(const Eigen::Ref<RUT::Vector7d> pose,
-                       double dt_in_future_ms = 1000);
-  void set_force_controlled_axis(const RUT::Matrix6d& Tr, int n_af);
-  void set_stiffness_matrix(const RUT::Matrix6d& stiffness);
+                       double dt_in_future_ms = 1000, int robot_id = 0);
+  void set_force_controlled_axis(const RUT::Matrix6d& Tr, int n_af,
+                                 int robot_id = 0);
+  void set_stiffness_matrix(const RUT::Matrix6d& stiffness, int robot_id = 0);
 
   void schedule_waypoints(const Eigen::MatrixXd& waypoints,
-                          const Eigen::VectorXd& timepoints_ms);
+                          const Eigen::VectorXd& timepoints_ms,
+                          int robot_id = 0);
   void schedule_stiffness(const Eigen::MatrixXd& stiffness,
-                          const Eigen::VectorXd& timepoints_ms);
+                          const Eigen::VectorXd& timepoints_ms,
+                          int robot_id = 0);
 
   void clear_cmd_buffer();
 
@@ -132,82 +142,95 @@ class ManipServer {
   void stop_saving_data();
   bool is_saving_data();
 
+  bool is_bimanual() { return _config.bimanual; }
+
  private:
   // config
   ManipServerConfig _config;
 
+  // list of id
+  std::vector<int> _id_list;
+
   // data buffer
-  RUT::DataBuffer<Eigen::MatrixXd> _camera_rgb_buffer;
-  RUT::DataBuffer<Eigen::VectorXd> _pose_buffer;
-  RUT::DataBuffer<Eigen::VectorXd> _wrench_buffer;
+  std::vector<RUT::DataBuffer<Eigen::MatrixXd>> _camera_rgb_buffers;
+  std::vector<RUT::DataBuffer<Eigen::VectorXd>> _pose_buffers;
+  std::vector<RUT::DataBuffer<Eigen::VectorXd>> _wrench_buffers;
   // action buffer
-  RUT::DataBuffer<Eigen::VectorXd> _waypoints_buffer;
-  RUT::DataBuffer<Eigen::MatrixXd> _stiffness_buffer;
+  std::vector<RUT::DataBuffer<Eigen::VectorXd>> _waypoints_buffers;
+  std::vector<RUT::DataBuffer<Eigen::MatrixXd>> _stiffness_buffers;
 
-  RUT::DataBuffer<double> _camera_rgb_timestamp_ms_buffer;
-  RUT::DataBuffer<double> _pose_timestamp_ms_buffer;
-  RUT::DataBuffer<double> _wrench_timestamp_ms_buffer;
-  RUT::DataBuffer<double> _waypoints_timestamp_ms_buffer;
-  RUT::DataBuffer<double> _stiffness_timestamp_ms_buffer;
+  std::vector<RUT::DataBuffer<double>> _camera_rgb_timestamp_ms_buffers;
+  std::vector<RUT::DataBuffer<double>> _pose_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>> _stiffness_timestamp_ms_buffers;
 
-  std::mutex _camera_rgb_buffer_mtx;
-  std::mutex _pose_buffer_mtx;
-  std::mutex _wrench_buffer_mtx;
-  std::mutex _waypoints_buffer_mtx;
-  std::mutex _stiffness_buffer_mtx;
+  std::deque<std::mutex> _camera_rgb_buffer_mtxs;
+  std::deque<std::mutex> _pose_buffer_mtxs;
+  std::deque<std::mutex> _wrench_buffer_mtxs;
+  std::deque<std::mutex> _waypoints_buffer_mtxs;
+  std::deque<std::mutex> _stiffness_buffer_mtxs;
 
   // timing
   RUT::Timer _timer;
 
   // additional configs as local variables
-  RUT::Matrix6d _stiffness_high{};
-  RUT::Matrix6d _stiffness_low{};
+  std::vector<RUT::Matrix6d> _stiffnesses_high{};
+  std::vector<RUT::Matrix6d> _stiffnesses_low{};
 
   //  hardware interfaces
-  std::shared_ptr<CameraInterfaces> camera_ptr;
-  std::shared_ptr<FTInterfaces> force_sensor_ptr;
-  std::shared_ptr<RobotInterfaces> robot_ptr;
-  AdmittanceController controller;
-  PerturbationGenerator perturbation_generator;
+  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;
+  // controllers
+  std::vector<AdmittanceController> _controllers;
+  std::vector<PerturbationGenerator> _perturbation_generators;
+  std::deque<std::mutex> _controller_mtxs;
 
   // threads
-  std::thread _rgb_thread;
-  std::thread _low_dim_thread;
+  std::vector<std::thread> _robot_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::string _ctrl_rgb_folder;
-  std::ofstream _ctrl_low_dim_data_stream;
+  std::vector<std::string> _ctrl_rgb_folders;
+  std::vector<std::ofstream> _ctrl_robot_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;
 
-  // controller variables to control the force controller
-  std::mutex _controller_mtx;
-
   // state variable indicating the status of the threads
-  bool _state_low_dim_thread_ready = false;
-  bool _state_rgb_thread_ready = false;
-  bool _state_low_dim_thread_saving = false;
-  bool _state_rgb_thread_saving = false;
-  int _state_low_dim_seq_id = 0;
-  int _state_rgb_seq_id = 0;
-
-  // pre-allocated variables for camera feedback
-  cv::Mat _color_mat;  // also used for plot loop, so need a lock
-  std::mutex _color_mat_mtx;
-  cv::Mat* _bgr;  //destination array
-  Eigen::MatrixXd _bm, _gm, _rm;
-  Eigen::MatrixXd _rgb_row_combined;
+  std::vector<bool> _states_robot_thread_ready{};
+  std::vector<bool> _states_rgb_thread_ready{};
+  std::vector<bool> _states_wrench_thread_ready{};
+  bool _state_plot_thread_ready{false};
 
-  // temp variables storing timestamps of data just being fetched
-  Eigen::VectorXd _camera_rgb_timestamps_ms;
-  Eigen::VectorXd _pose_timestamps_ms;
-  Eigen::VectorXd _wrench_timestamps_ms;
+  std::vector<bool> _states_robot_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_rgb_seq_id{};
+  std::vector<int> _states_wrench_seq_id{};
 
-  void low_dim_loop(const RUT::TimePoint& time0);
+  // shared variables between camera thread and plot thread
+  std::vector<cv::Mat> _color_mats;
+  std::deque<std::mutex> _color_mat_mtxs;
 
-  void rgb_loop(const RUT::TimePoint& time0);
+  // shared variables between robot thread and wrench thread
+  std::vector<Eigen::VectorXd> _poses_fb;
+  std::deque<std::mutex> _poses_fb_mtxs;
 
-  void rgb_plot_loop();
+  // 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> _wrench_timestamps_ms;
+
+  void robot_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);
+  void rgb_plot_loop();  // opencv plotting does not support multi-threading
 };

workcell/table_top_manip/python/test.py

@@ -1,4 +1,4 @@
-import manip_server_pybind as hs
+import manip_server_pybind as ms
 import numpy as np
 from time import sleep
 import cv2
@@ -7,8 +7,10 @@
 
 print("[python] creating manip server")
 
-server = hs.ManipServer(
-    "/home/yifanhou/git/RobotTestBench/applications/ur_test_bench/config/ur_test_bench.yaml"
+server = ms.ManipServer()
+server.initialize(
+    "/home/yifanhou/git/RobotTestBench/applications/ur_test_bench/config/ur_1.yaml"
+    # "/home/yifanhou/git/RobotTestBench/applications/ur_test_bench/config/ur_test_bench.yaml"
 )
 
 print("[python] server created")
@@ -21,7 +23,6 @@
 print("[python] pose_fb:", pose_fb)
 
 # server.set_high_level_maintain_position()
-
 # bgr = np.zeros((1080, 1080, 3), dtype=np.uint8)
 # cv2.namedWindow("image")
 
@@ -59,7 +60,9 @@
     for j in range(5):
         pose_cmds[:, j] = pose_fb.reshape((7,))
         pose_cmds[0, j] += deltas[i] * j
-    timepoints_ms = np.array([0.0, 100, 200, 300, 400]) + server.get_timestamp_now_ms()
+    timepoints_ms = (
+        np.array([0.0, 400, 800, 1200, 1600]) + server.get_timestamp_now_ms()
+    )
     server.schedule_waypoints(pose_cmds, timepoints_ms)
     pose_fb = server.get_pose(1)
     log_pose_x.append(pose_fb[0])
@@ -70,7 +73,7 @@
     log_pose_cmd_y.append(pose_cmd[1])
     log_pose_cmd_z.append(pose_cmd[2])
 
-    sleep(0.2)
+    sleep(5)
 
 sleep(0.5)
 server.join_threads()

workcell/table_top_manip/python/test_bimanual.py

@@ -0,0 +1,103 @@
+import manip_server_pybind as ms
+import numpy as np
+from time import sleep
+import cv2
+import copy
+import matplotlib.pyplot as plt
+
+print("[python] creating manip server")
+
+server = ms.ManipServer()
+if not server.initialize(
+    "/home/yifanhou/git/RobotTestBench/applications/ur_test_bench/config/ur_test_bench.yaml"
+):
+    print("[python] failed to initialize server")
+    exit(1)
+
+print("[python] server created")
+while not server.is_ready():
+    print("[python] waiting for server to be ready")
+    sleep(1)
+print("[python] Server is ready")
+
+pose0_fb = server.get_pose(1, 0)
+pose1_fb = server.get_pose(1, 1)
+
+server.set_high_level_maintain_position()
+
+
+# bgr = np.zeros((1080, 1080, 3), dtype=np.uint8)
+# cv2.namedWindow("image")
+
+server.set_force_controlled_axis(np.eye(6), 6)
+print("[python] done set_force_controlled_axis")
+sleep(10)
+exit(0)
+
+
+pose_cmd = copy.deepcopy(pose_fb)
+pose_cmds = np.zeros((7, 5))
+
+log_pose_x = []
+log_pose_y = []
+log_pose_z = []
+
+log_pose_cmd_x = []
+log_pose_cmd_y = []
+log_pose_cmd_z = []
+
+deltas = np.array([0.01, -0.01])
+for i in range(2):
+    wrench = server.get_wrench(1)
+    pose_fb = server.get_pose(1)
+
+    # rgb_row_combined = server.get_camera_rgb()
+    # # rgb = rgb_row_combined.reshape((1080, 1080, 3))
+    # # # rgb to bgr
+    # # bgr = rgb[:, :, ::-1]
+    # bgr[:, :, 2] = rgb_row_combined[0:1080, 0:1080]
+    # bgr[:, :, 1] = rgb_row_combined[1080:2160, 0:1080]
+    # bgr[:, :, 0] = rgb_row_combined[2160:3240, 0:1080]
+    # # print(f"image size: {rgb_row_combined.shape}, rgb size: {rgb.shape}")
+
+    # cv2.imshow("image", bgr)
+    # key = cv2.waitKey(20)
+
+    for j in range(5):
+        pose_cmds[:, j] = pose_fb.reshape((7,))
+        pose_cmds[0, j] += deltas[i] * j
+    timepoints_ms = (
+        np.array([0.0, 400, 800, 1200, 1600]) + server.get_timestamp_now_ms()
+    )
+    server.schedule_waypoints(pose_cmds, timepoints_ms)
+    pose_fb = server.get_pose(1)
+    log_pose_x.append(pose_fb[0])
+    log_pose_y.append(pose_fb[1])
+    log_pose_z.append(pose_fb[2])
+
+    log_pose_cmd_x.append(pose_cmd[0])
+    log_pose_cmd_y.append(pose_cmd[1])
+    log_pose_cmd_z.append(pose_cmd[2])
+
+    sleep(5)
+
+sleep(0.5)
+server.join_threads()
+print("start drawing")
+
+# fig, axs = plt.subplots(1, 3, figsize=(9, 3), sharey=True)
+
+# # plot log_pose_x, log_pose_y, log_pose_z
+# axs[0].plot(log_pose_x, label="pose_x")
+# axs[1].plot(log_pose_y, label="pose_y")
+# axs[2].plot(log_pose_z, label="pose_z")
+
+# axs[0].plot(log_pose_cmd_x, label="pose_cmd_x")
+# axs[1].plot(log_pose_cmd_y, label="pose_cmd_y")
+# axs[2].plot(log_pose_cmd_z, label="pose_cmd_z")
+
+# axs[0].legend()
+# axs[1].legend()
+# axs[2].legend()
+
+# plt.show()