Merge remote-tracking branch 'origin/dev' into dev

src/robot/fracture_robot/fracture_robot_interface.cpp

@@ -47,7 +47,7 @@ namespace sophiar {
         static constexpr auto robot_endian = boost::endian::order::little;
         static constexpr auto default_max_velocity = 5.0; // mm/s
         static constexpr auto default_controller_gain = 5.0;
-        static constexpr size_t control_packet_length = 6 * sizeof(int32_t) + 2;
+        static constexpr size_t control_packet_length = 6 * sizeof(int32_t);
         static constexpr uint16_t control_packet_tail = 0x7fff;
 
         struct screw_info {
@@ -74,9 +74,9 @@ namespace sophiar {
 
         void fake_inverse_kinematics(const transform_obj::value_type &trans,
                                      array6_obj::value_type &pos) const {
-            static constexpr auto side_z_offset = -111.6; // mm
-            static constexpr auto top1_y_offset = 429.3;
-            static constexpr auto top2_y_offset = 569.3;
+            static constexpr auto side_z_offset = -60.0; // mm
+            static constexpr auto top1_y_offset = 440.0;
+            static constexpr auto top2_y_offset = 590.0;
 
             { // side cross
                 auto origin = trans * side_screw.origin;
@@ -84,8 +84,8 @@ namespace sophiar {
                 auto t = (side_z_offset - origin.z()) / direction.z();
                 auto cross = origin + t * direction;
                 assert(std::abs(cross.z() - side_z_offset) < 1e-3);
-                pos[0] = cross.x(); // +X
-                pos[3] = -cross.y(); // -Y
+                pos[0] = cross.x();
+                pos[3] = cross.y();
             }
             { // top cross
                 auto origin = trans * top_screw.origin;
@@ -96,10 +96,10 @@ namespace sophiar {
                 auto cross2 = origin + t2 * direction;
                 assert(std::abs(cross1.y() - top1_y_offset) < 1e-3);
                 assert(std::abs(cross2.y() - top2_y_offset) < 1e-3);
-                pos[4] = cross1.x(); // +X
-                pos[5] = -cross1.z(); // -Z
-                pos[2] = cross2.x(); // +X
-                pos[1] = -cross2.z(); // -Z
+                pos[4] = -cross1.x();
+                pos[5] = -cross1.z();
+                pos[2] = -cross2.x();
+                pos[1] = -cross2.z();
             }
         }
 
@@ -107,7 +107,7 @@ namespace sophiar {
                                control_value_type &arr_val) {
             static constexpr int base_freq = 1000000; // Hz
             static constexpr auto dis_per_clock = 0.00625; // mm
-            static constexpr auto minimum_velocity = 0.05; // mm/s, lower than this will cause stop
+            static constexpr auto minimum_velocity = 0.5; // mm/s, lower than this will cause stop
 
             for (int i = 0; i < 6; ++i) {
                 if (std::abs(vec_val[i]) < minimum_velocity) {
@@ -126,7 +126,7 @@ namespace sophiar {
         void calc_target_velocity(const array6_obj::value_type &cur_pos,
                                   const array6_obj::value_type &tar_pos,
                                   array6_obj::value_type &vec_val) const {
-            static constexpr auto dis_tolerance = 0.1; // 0.1mm, lower than this will cause stop
+            static constexpr auto dis_tolerance = 0.5; // 0.1mm, lower than this will cause stop
 
             double cur_max_vec = 0;
             for (int i = 0; i < 6; ++i) {
@@ -163,7 +163,6 @@ namespace sophiar {
                     is_moving = true;
                 }
             }
-            writer << control_packet_tail;
 
             // send packet
             assert(conn->is_open());

tests/data/fracture_robot_interface_config.json

@@ -1,6 +1,34 @@
 {
   "controller_port": 5277,
   "variable_list": [
+    {
+      "name": "ref_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "bone_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "robot_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "target_in_robot",
+      "type": "transform_obj"
+    },
+    {
+      "name": "bone_in_robot",
+      "type": "transform_obj"
+    },
+    {
+      "name": "bone_in_ref",
+      "type": "transform_obj"
+    },
+    {
+      "name": "collect_result",
+      "type": "transform_obj"
+    },
     {
       "name": "robot_control_mode",
       "type": "u64int_obj"
@@ -8,13 +36,122 @@
     {
       "name": "robot_is_moving",
       "type": "bool_obj"
-    },
-    {
-      "name": "motor_velocity",
-      "type": "array6_obj"
     }
   ],
   "object_list": [
+    {
+      "type": "transform_tree",
+      "name": "transform_tree",
+      "init_config": {
+        "node_list": [
+          {
+            "name": "tracker"
+          },
+          {
+            "name": "ref",
+            "parent": "tracker",
+            "transform_var_name": "ref_in_tracker"
+          },
+          {
+            "name": "bone_target",
+            "parent": "ref",
+            "transform": [
+              -35.592,
+              31.7571,
+              -0.923395,
+              0.991606,
+              0.05668,
+              0.0567023,
+              -0.101438
+            ]
+          },
+          {
+            "name": "bone",
+            "parent": "tracker",
+            "transform_var_name": "bone_in_tracker"
+          },
+          {
+            "name": "robot",
+            "parent": "tracker",
+            "transform_var_name": "robot_in_tracker"
+          }
+        ]
+      },
+      "start_config": {
+        "watch_list": [
+          {
+            "target": "bone_target",
+            "observer": "robot",
+            "transform_var_name": "target_in_robot"
+          },
+          {
+            "target": "bone",
+            "observer": "robot",
+            "transform_var_name": "bone_in_robot"
+          },
+          {
+            "target": "bone",
+            "observer": "ref",
+            "transform_var_name": "bone_in_ref"
+          }
+        ]
+      }
+    },
+    {
+      "type": "ndi_interface",
+      "name": "ndi",
+      "init_config": {
+        "address_type": "serial",
+        "ip": "10.0.0.5",
+        "tcp_port": 8765,
+        "com_port": "COM9",
+        "tool_list": [
+          {
+            "rom_path": "D:\\Program\\Robot\\Tools\\roms\\TZL_Ref.rom",
+            "outputs": {
+              "transform": "ref_in_tracker"
+            }
+          },
+          {
+            "rom_path": "D:\\Program\\Robot\\Tools\\roms\\TZL_Bone.rom",
+            "outputs": {
+              "transform": "bone_in_tracker"
+            }
+          },
+          {
+            "rom_path": "D:\\Program\\Robot\\Tools\\roms\\TZB_Robot.rom",
+            "outputs": {
+              "transform": "robot_in_tracker"
+            }
+          }
+        ]
+      },
+      "start_config": {
+        "allow_unreliable": true,
+        "prefer_stream_tracking": false
+      }
+    },
+    {
+      "type": "transform_obj_validity_watcher",
+      "name": "ref_visibility_watcher",
+      "start_config": {
+        "variable_name": "ref_in_tracker"
+      }
+    },
+    {
+      "type": "transform_obj_validity_watcher",
+      "name": "bone_visibility_watcher",
+      "start_config": {
+        "variable_name": "bone_in_tracker"
+      }
+    },
+    {
+      "type": "transform_obj_validity_watcher",
+      "name": "robot_visibility_watcher",
+      "start_config": {
+        "variable_name": "robot_in_tracker"
+      }
+    },
     {
       "type": "fracture_robot_interface",
       "name": "frac_robot",
@@ -23,26 +160,26 @@
         "screw": {
           "top": {
             "origin": [
-              0,
-              0,
-              0
+              193.62,
+              274.24,
+              -169.95
             ],
             "direction": [
-              0,
-              1,
-              0
+              0.2580,
+              -0.8932,
+              -0.3682
             ]
           },
           "side": {
             "origin": [
-              0,
-              0,
-              0
+              225.42,
+              262.80,
+              -195.37
             ],
             "direction": [
-              0,
-              0,
-              -1
+              0.9044,
+              0.4147,
+              0.1004
             ]
           }
         }
@@ -50,17 +187,60 @@
       "start_config": {
         "input_config": {
           "max_velocity": 5.0,
-          "controller_gain": 3.0,
-          "control_mode": "direct",
-          "current_tcp": "tcp_in_robot_base",
-          "target_tcp": "target_tcp_in_robot_base",
-          "target_motor_velocity": "motor_velocity"
+          "controller_gain": 0.5,
+          "control_mode": "cartesian",
+          "current_tcp": "bone_in_robot",
+          "target_tcp": "target_in_robot"
         },
         "output_config": {
           "current_control_mode": "robot_control_mode",
           "is_moving": "robot_is_moving"
         }
+      },
+      "dependencies": [
+        "transform_tree",
+        "ndi"
+      ]
+    },
+    {
+      "type": "empty_object",
+      "name": "all",
+      "dependencies": [
+        "frac_robot",
+        "ref_visibility_watcher",
+        "bone_visibility_watcher",
+        "robot_visibility_watcher"
+      ]
+    },
+    {
+      "type": "transform_stabilizer",
+      "name": "target_collector",
+      "start_config": {
+        "stable_type": "transform",
+        "input_var_name": "bone_in_ref",
+        "output_var_name": "collect_result",
+        "linear_tolerance_mm": 0.05,
+        "temporal_interval_s": 3
+      },
+      "dependencies": [
+        "ndi"
+      ]
+    },
+    {
+      "type": "transform_obj_watcher",
+      "name": "result_watcher",
+      "start_config": {
+        "variable_name": "collect_result"
       }
+    },
+    {
+      "type": "empty_object",
+      "name": "collect",
+      "dependencies": [
+        "target_collector",
+        "result_watcher",
+        "transform_tree"
+      ]
     }
   ]
 }