Bimanual is working: varies bug fixes and improvements during testing

* Seperate wrench from robot thread so it can run at a different frequency
* Make each thread optional from config
* Fixed plotting thread freeze by restricting plotting to one thread and
  combining all images into one
* Fixed default parameter in pybind
* Varies bug fixes for mock hardware mode
* Added python test script for bimanual
* Varies bug fixes for jerky motion
* Added profiling and lots of printout about timing

Test: tested in experiments

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

@@ -33,8 +33,10 @@
 
 struct ManipServerConfig {
   std::string data_folder{""};
-  bool plot_rgb{false};
   bool run_robot_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 wrench_buffer_size{100};
@@ -47,8 +49,10 @@ struct ManipServerConfig {
   bool deserialize(const YAML::Node& node) {
     try {
       data_folder = node["data_folder"].as<std::string>();
-      plot_rgb = node["plot_rgb"].as<bool>();
       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>();
       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>();
@@ -138,6 +142,8 @@ class ManipServer {
   void stop_saving_data();
   bool is_saving_data();
 
+  bool is_bimanual() { return _config.bimanual; }
+
  private:
   // config
   ManipServerConfig _config;
@@ -185,7 +191,7 @@ class ManipServer {
   std::vector<std::thread> _robot_threads;
   std::vector<std::thread> _wrench_threads;
   std::vector<std::thread> _rgb_threads;
-  std::vector<std::thread> _rgb_plot_threads;
+  std::thread _rgb_plot_thread;
 
   // control variables to control the threads
   std::vector<std::string> _ctrl_rgb_folders;
@@ -199,6 +205,7 @@ class ManipServer {
   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};
 
   std::vector<bool> _states_robot_thread_saving{};
   std::vector<bool> _states_rgb_thread_saving{};
@@ -225,5 +232,5 @@ class ManipServer {
   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(int camera_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()

workcell/table_top_manip/src/helpers.hpp

@@ -46,6 +46,12 @@ inline void create_folder_for_new_episode(
     rgb_folders.push_back(rgb_folder);
     robot_json_files.push_back(robot_json_file);
     wrench_json_files.push_back(wrench_json_file);
+    std::cout << "[create_folder_for_new_episode] Created rgb folder: "
+              << rgb_folder << std::endl;
+    std::cout << "[create_folder_for_new_episode] generated robot file: "
+              << robot_json_file << std::endl;
+    std::cout << "[create_folder_for_new_episode] generated wrench file: "
+              << wrench_json_file << std::endl;
   }
 }