Implemented kuka navigation.

app/data/kuka_calibration_config.json

@@ -74,7 +74,7 @@
         "com_port": "COM3",
         "tool_list": [
           {
-            "rom_path": "/home/tpx/data/roms/Drill_3Ball.rom",
+            "rom_path": "/home/tpx/data/roms/Probe_Small_4Ball.rom",
             "outputs": {
               "transform": "ref_in_tracker"
             }

app/data/kuka_navigation.json

@@ -0,0 +1,495 @@
+{
+  "controller_port": 5277,
+  "variable_list": [
+    {
+      "name": "probe_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "model_ref_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "model_in_model_ref",
+      "type": "transform_obj",
+      "value": [
+        135.945,
+        39.1189,
+        -267.07,
+        0.536927,
+        0.60565,
+        -0.471815,
+        -0.349698
+      ]
+    },
+    {
+      "name": "probe_in_model_ref",
+      "type": "transform_obj"
+    },
+    {
+      "name": "probe_in_model",
+      "type": "transform_obj"
+    },
+    {
+      "name": "tool_in_flange_const",
+      "type": "transform_obj",
+      "value": [
+        193.183270697813,
+        119.298696093691,
+        1.38808397963203,
+        0.341548509981856,
+        -0.342944503560901,
+        -0.618995814406514,
+        0.618528790400057
+      ]
+    },
+    {
+      "name": "tool_in_model",
+      "type": "transform_obj"
+    },
+    {
+      "name": "robot_flange_in_base",
+      "type": "transform_obj"
+    },
+    {
+      "name": "tip_in_model_ref",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "picked_point_in_model_ref",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "model_in_model_ref_error",
+      "type": "double_obj"
+    },
+    {
+      "name": "tip_in_model",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "picked_point_in_model",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "target_tool_in_model",
+      "type": "transform_obj"
+    },
+    {
+      "name": "target_tool_offset",
+      "type": "transform_obj"
+    },
+    {
+      "name": "target_flange_offset",
+      "type": "transform_obj"
+    },
+    {
+      "name": "target_flange_pose_temp",
+      "type": "transform_obj"
+    },
+    {
+      "name": "flange_move_sig",
+      "type": "bool_obj"
+    },
+    {
+      "name": "target_flange_pose",
+      "type": "transform_obj"
+    },
+    {
+      "name": "robot_flange_speed_limit",
+      "type": "double_obj",
+      "value": 5
+    },
+    {
+      "name": "kuka_joint_speed_limit",
+      "type": "array7_obj",
+      "value": [
+        0.1,
+        0.1,
+        0.1,
+        0.1,
+        0.1,
+        0.1,
+        0.1
+      ]
+    },
+    {
+      "name": "robot_is_moving",
+      "type": "bool_obj"
+    },
+    {
+      "name": "kuka_control_mode",
+      "type": "u64int_obj"
+    }
+  ],
+  "object_list": [
+    {
+      "type": "transform_tree",
+      "name": "transform_tree",
+      "init_config": {
+        "node_list": [
+          {
+            "name": "tracker"
+          },
+          {
+            "name": "probe",
+            "parent": "tracker",
+            "transform_var_name": "probe_in_tracker"
+          },
+          {
+            "name": "model_ref",
+            "parent": "tracker",
+            "transform_var_name": "model_ref_in_tracker"
+          },
+          {
+            "name": "model",
+            "parent": "model_ref",
+            "transform_var_name": "model_in_model_ref"
+          },
+          {
+            "name": "robot_base",
+            "parent": "tracker",
+            "transform": [
+              249.608683668030,
+              378.363630568367,
+              -1999.86036620645,
+              0.694408974678311,
+              0.320622130095823,
+              -0.539876883852255,
+              -0.351469167724585
+            ]
+          },
+          {
+            "name": "robot_flange",
+            "parent": "robot_base",
+            "transform_var_name": "robot_flange_in_base"
+          },
+          {
+            "name": "tool_real",
+            "parent": "robot_flange",
+            "transform_var_name": "tool_in_flange_const"
+          },
+          {
+            "name": "tool",
+            "parent": "tool_real",
+            "transform": [
+              0,
+              0,
+              -30,
+              1,
+              0,
+              0,
+              0
+            ]
+          }
+        ]
+      },
+      "start_config": {
+        "watch_list": [
+          {
+            "target": "probe",
+            "observer": "model_ref",
+            "transform_var_name": "probe_in_model_ref"
+          },
+          {
+            "target": "probe",
+            "observer": "model",
+            "transform_var_name": "probe_in_model"
+          },
+          {
+            "target": "tool",
+            "observer": "model",
+            "transform_var_name": "tool_in_model"
+          }
+        ]
+      }
+    },
+    {
+      "type": "ndi_interface",
+      "name": "ndi",
+      "init_config": {
+        "address_type": "ethernet",
+        "ip": "10.0.0.5",
+        "tcp_port": 8765,
+        "com_port": "COM3",
+        "tool_list": [
+          {
+            "rom_path": "/home/tpx/data/roms/Probe_Small_3Ball.rom",
+            "outputs": {
+              "transform": "probe_in_tracker"
+            }
+          },
+          {
+            "rom_path": "/home/tpx/data/roms/Drill_3Ball.rom",
+            "outputs": {
+              "transform": "model_ref_in_tracker"
+            }
+          }
+        ]
+      },
+      "start_config": {
+        "allow_unreliable": true,
+        "prefer_stream_tracking": false
+      }
+    },
+    {
+      "type": "transform_obj_validity_watcher",
+      "name": "probe_visibility_watcher",
+      "start_config": {
+        "variable_name": "probe_in_tracker"
+      }
+    },
+    {
+      "type": "transform_obj_validity_watcher",
+      "name": "model_ref_visibility_watcher",
+      "start_config": {
+        "variable_name": "model_ref_in_tracker"
+      }
+    },
+    {
+      "type": "scalarxyz_transformer",
+      "name": "probe_tip_transformer_for_reg",
+      "start_config": {
+        "transform_type": "point",
+        "transform_var_name": "probe_in_model_ref",
+        "target_value": [
+          0,
+          0,
+          0
+        ],
+        "output_var_name": "tip_in_model_ref"
+      },
+      "dependencies": [
+        "ndi",
+        "transform_tree"
+      ]
+    },
+    {
+      "type": "transform_stabilizer",
+      "name": "point_picker_for_reg",
+      "start_config": {
+        "stable_type": "point",
+        "input_var_name": "tip_in_model_ref",
+        "output_var_name": "picked_point_in_model_ref",
+        "linear_tolerance_mm": 0.05,
+        "temporal_interval_s": 3
+      },
+      "dependencies": [
+        "probe_tip_transformer_for_reg"
+      ]
+    },
+    {
+      "type": "scalarxyz_obj_watcher",
+      "name": "picked_point_watcher_for_reg",
+      "start_config": {
+        "variable_name": "picked_point_in_model_ref"
+      }
+    },
+    {
+      "type": "landmark_registration",
+      "name": "model_registrator",
+      "start_config": {
+        "fiducial_points": [
+          [
+            9.16363,
+            245.77,
+            -35.5363
+          ],
+          [
+            -38.2517,
+            238.265,
+            15.8886
+          ],
+          [
+            -18.4721,
+            228.506,
+            -24.2903
+          ]
+        ],
+        "point_var_name": "picked_point_in_model_ref",
+        "transform_var_name": "model_in_model_ref",
+        "error_var_name": "model_in_model_ref_error"
+      },
+      "dependencies": [
+        "point_picker_for_reg"
+      ]
+    },
+    {
+      "type": "transform_obj_watcher",
+      "name": "registration_result_watcher",
+      "start_config": {
+        "variable_name": "model_in_model_ref"
+      }
+    },
+    {
+      "type": "double_obj_watcher",
+      "name": "registration_error_watcher",
+      "start_config": {
+        "variable_name": "model_in_model_ref_error"
+      }
+    },
+    {
+      "type": "scalarxyz_transformer",
+      "name": "probe_tip_transformer_for_nav",
+      "start_config": {
+        "transform_type": "point",
+        "transform_var_name": "probe_in_model",
+        "target_value": [
+          0,
+          0,
+          0
+        ],
+        "output_var_name": "tip_in_model"
+      },
+      "dependencies": [
+        "ndi",
+        "transform_tree"
+      ]
+    },
+    {
+      "type": "transform_stabilizer",
+      "name": "point_picker_for_icp",
+      "start_config": {
+        "stable_type": "point",
+        "input_var_name": "tip_in_model",
+        "output_var_name": "picked_point_in_model",
+        "linear_tolerance_mm": 0.05,
+        "temporal_interval_s": 2
+      },
+      "dependencies": [
+        "probe_tip_transformer_for_nav"
+      ]
+    },
+    {
+      "type": "scalarxyz_obj_watcher",
+      "name": "picked_point_watcher_for_icp",
+      "start_config": {
+        "variable_name": "picked_point_in_model"
+      }
+    },
+    {
+      "type": "kuka_interface",
+      "name": "kuka",
+      "init_config": {
+        "address": "10.0.0.10"
+      },
+      "start_config": {
+        "output_config": {
+          "report_frequency": 60,
+          "flange_info": {
+            "pose": "robot_flange_in_base"
+          },
+          "is_moving": "robot_is_moving",
+          "current_control_mode": "kuka_control_mode"
+        },
+        "input_config": {
+          "control_mode": "hard_flange_pose",
+          "flange_pose": "target_flange_pose",
+          "flange_speed_limit": "robot_flange_speed_limit",
+          "joint_speed_limit": "kuka_joint_speed_limit"
+        }
+      }
+    },
+    {
+      "type": "five_dof_offset_calculator",
+      "name": "tcp_offset_calculator",
+      "start_config": {
+        "current_transform_var": "tool_in_model",
+        "target_transform_var": "target_tool_in_model",
+        "offset_transform_var": "target_tool_offset"
+      },
+      "dependencies": [
+        "kuka",
+        "ndi",
+        "transform_tree"
+      ]
+    },
+    {
+      "type": "tcp_offset_to_flange_calculator",
+      "name": "flange_offset_calculator",
+      "start_config": {
+        "tcp_offset_transform_var": "target_tool_offset",
+        "tcp_in_flange_transform_var": "tool_in_flange_const",
+        "flange_offset_transform_var": "target_flange_offset"
+      },
+      "dependencies": [
+        "kuka",
+        "tcp_offset_calculator"
+      ]
+    },
+    {
+      "type": "simple_offset_to_pose_controller",
+      "name": "kuka_controller",
+      "start_config": {
+        "current_transform_var": "robot_flange_in_base",
+        "offset_transform_var": "target_flange_offset",
+        "target_transform_var": "target_flange_pose_temp"
+      },
+      "dependencies": [
+        "kuka",
+        "flange_offset_calculator"
+      ]
+    },
+    {
+      "type": "transform_obj_watcher",
+      "name": "robot_old_flange_watcher",
+      "start_config": {
+        "variable_name": "robot_flange_in_base",
+        "minimum_interval_ms": 2000
+      }
+    },
+    {
+      "type": "transform_obj_watcher",
+      "name": "robot_new_flange_watcher",
+      "start_config": {
+        "variable_name": "target_flange_pose_temp",
+        "minimum_interval_ms": 2000
+      }
+    },
+    {
+      "type": "transform_obj_d_latch",
+      "name": "kuka_controller_latch",
+      "start_config": {
+        "input_name": "target_flange_pose_temp",
+        "output_name": "target_flange_pose",
+        "signal_name": "flange_move_sig"
+      },
+      "dependencies": [
+        "kuka",
+        "kuka_controller"
+      ]
+    },
+    {
+      "type": "empty_object",
+      "name": "reg_model",
+      "dependencies": [
+        "probe_visibility_watcher",
+        "model_ref_visibility_watcher",
+        "picked_point_watcher_for_reg",
+        "model_registrator",
+        "registration_result_watcher",
+        "registration_error_watcher"
+      ]
+    },
+    {
+      "type": "empty_object",
+      "name": "icp_model",
+      "dependencies": [
+        "probe_visibility_watcher",
+        "model_ref_visibility_watcher",
+        "picked_point_watcher_for_icp",
+        "point_picker_for_icp",
+        "registration_result_watcher"
+      ]
+    },
+    {
+      "type": "empty_object",
+      "name": "robot",
+      "dependencies": [
+        "kuka_controller_latch",
+        "robot_old_flange_watcher",
+        "robot_new_flange_watcher"
+      ]
+    }
+  ]
+}

app/kuka_calibration.cpp

@@ -26,7 +26,7 @@ using boost::asio::detached;
 
 awaitable<void> collect_data() {
     static constexpr auto resolution = 32;
-    static constexpr auto move_angle = 0.5;
+    static constexpr auto move_angle = 0.3;
     static constexpr auto move_distance = 75;
 
     static constexpr auto pi = std::numbers::pi;
@@ -79,7 +79,8 @@ int main() {
     std::ifstream config_file("data/kuka_calibration_config.json");
     assert(config_file.is_open());
     auto config = nlohmann::json::parse(config_file);
-    assert(initialize(config));
+    auto ok = initialize(config);
+    assert(ok);
     co_spawn(*global_context, collect_data(), detached);
     global_context->run();
 }

src/algorithm/five_dof_offset_calculator.hpp

@@ -0,0 +1,58 @@
+#ifndef SOPHIAR2_FIVE_DOF_OFFSET_CALCULATOR_HPP
+#define SOPHIAR2_FIVE_DOF_OFFSET_CALCULATOR_HPP
+
+#include "core/basic_obj_types.hpp"
+#include "utility/config_utility.hpp"
+#include "utility/coro_signal_group.hpp"
+#include "utility/coro_worker.hpp"
+#include "utility/simple_tristate_obj.hpp"
+#include "utility/variable_helper.hpp"
+
+#include "utility/assert_utility.h"
+
+namespace sophiar {
+
+    // Fix origin and Z-axis only
+    inline coro_worker::pointer make_5dof_offset_calculator_func(const nlohmann::json &config) {
+        auto cur_index = LOAD_VARIABLE_INDEX(transform_obj, "current_transform_var");
+        auto tar_index = LOAD_VARIABLE_INDEX(transform_obj, "target_transform_var");
+        auto offset_index = LOAD_VARIABLE_INDEX(transform_obj, "offset_transform_var");
+
+        auto signal_group = coro_signal_any_group::new_instance();
+        signal_group->add_watcher(REQUIRE_VARIABLE_WATCHER(cur_index));
+        signal_group->add_watcher(REQUIRE_VARIABLE_WATCHER(tar_index));
+        auto signal_watcher = signal_group->new_watcher();
+        signal_group->start();
+        auto exit_func = SIGNAL_GROUP_AUTO_CLOSER(signal_group);
+
+        auto worker = make_infinite_coro_worker(
+                [=,
+                        watcher = std::move(signal_watcher)]() mutable
+                        -> boost::asio::awaitable<bool> {
+                    co_await watcher.coro_wait(false);
+                    auto ts = watcher.get_last_update_ts();
+                    auto cur_trans = QUERY_VARIABLE(transform_obj, cur_index);
+                    auto tar_trans = QUERY_VARIABLE(transform_obj, tar_index);
+                    if (cur_trans == nullptr || tar_trans == nullptr) [[unlikely]] {
+                        UPDATE_VARIABLE_WITH_TS(transform_obj, offset_index, nullptr, ts);
+                    } else [[likely]] {
+                        auto z_axis = Eigen::Vector3d::UnitZ();
+                        auto cur_z_axis = cur_trans->value.linear() * z_axis;
+                        auto tar_z_axis = tar_trans->value.linear() * z_axis;
+                        auto rot_part = Eigen::Quaterniond::FromTwoVectors(cur_z_axis, tar_z_axis);
+                        auto trans_part = Eigen::Translation3d{
+                                tar_trans->value.translation() - cur_trans->value.translation()};
+                        auto offset = transform_obj::new_instance();
+                        offset->value = trans_part * rot_part;
+                        offset->value = cur_trans->value.inverse() * offset->value * cur_trans->value;
+                        UPDATE_VARIABLE_WITH_TS(transform_obj, offset_index, offset, ts);
+                    }
+                    co_return true;
+                }, std::move(exit_func));
+
+        return std::move(worker);
+    }
+
+}
+
+#endif //SOPHIAR2_FIVE_DOF_OFFSET_CALCULATOR_HPP

src/algorithm/tcp_offset_to_flange_calculator.hpp

@@ -0,0 +1,45 @@
+#ifndef SOPHIAR2_TCP_OFFSET_TO_FLANGE_CALCULATOR_HPP
+#define SOPHIAR2_TCP_OFFSET_TO_FLANGE_CALCULATOR_HPP
+
+#include "core/basic_obj_types.hpp"
+#include "utility/config_utility.hpp"
+#include "utility/coro_signal_group.hpp"
+#include "utility/coro_worker.hpp"
+#include "utility/simple_tristate_obj.hpp"
+#include "utility/variable_helper.hpp"
+
+#include "utility/assert_utility.h"
+
+namespace sophiar {
+
+    // Change offset in TCP to offset in flange
+    // TODO Add support for fixed transform
+    inline coro_worker::pointer make_tcp_offset_to_flange_calculator_func(const nlohmann::json &config) {
+        auto input_index = LOAD_VARIABLE_INDEX(transform_obj, "tcp_offset_transform_var");
+        auto trans_index = LOAD_VARIABLE_INDEX(transform_obj, "tcp_in_flange_transform_var");
+        auto output_index = LOAD_VARIABLE_INDEX(transform_obj, "flange_offset_transform_var");
+
+        auto worker = make_infinite_coro_worker(
+                [=,
+                        input_helper = VARIABLE_MANUAL_DELEGATE(transform_obj, input_index)]() mutable
+                        -> boost::asio::awaitable<bool> {
+                    co_await input_helper.coro_wait_update();
+                    auto ts = input_helper.get_last_update_ts();
+                    auto trans = QUERY_VARIABLE(transform_obj, trans_index);
+                    if (input_helper.empty() || trans == nullptr) [[unlikely]] {
+                        UPDATE_VARIABLE_WITH_TS(transform_obj, output_index, nullptr, ts);
+                    } else [[likely]] {
+                        auto flange_offset = transform_obj::new_instance();
+                        flange_offset->value = trans->value
+                                               * input_helper->value
+                                               * trans->value.inverse();
+                        UPDATE_VARIABLE_WITH_TS(transform_obj, output_index, flange_offset, ts);
+                    }
+                    co_return true;
+                });
+        return std::move(worker);
+    }
+
+}
+
+#endif //SOPHIAR2_TCP_OFFSET_TO_FLANGE_CALCULATOR_HPP

src/algorithm/transform_utility.hpp

@@ -8,7 +8,7 @@
 #include "utility/simple_tristate_obj.hpp"
 #include "utility/variable_helper.hpp"
 
-#include <cassert>
+#include "utility/assert_utility.h"
 
 namespace sophiar {
 

src/controller/simple_offset_to_pose_controller.hpp

@@ -0,0 +1,41 @@
+#ifndef SOPHIAR2_SIMPLE_OFFSET_TO_POSE_CONTROLLER_HPP
+#define SOPHIAR2_SIMPLE_OFFSET_TO_POSE_CONTROLLER_HPP
+
+#include "core/basic_obj_types.hpp"
+#include "utility/config_utility.hpp"
+#include "utility/coro_signal_group.hpp"
+#include "utility/coro_worker.hpp"
+#include "utility/simple_tristate_obj.hpp"
+#include "utility/variable_helper.hpp"
+
+#include "utility/assert_utility.h"
+
+namespace sophiar {
+
+    // Imply to offset to pose
+    inline coro_worker::pointer make_simple_offset_to_pose_controller_func(const nlohmann::json &config) {
+        auto cur_index = LOAD_VARIABLE_INDEX(transform_obj, "current_transform_var");
+        auto offset_index = LOAD_VARIABLE_INDEX(transform_obj, "offset_transform_var");
+        auto tar_index = LOAD_VARIABLE_INDEX(transform_obj, "target_transform_var");
+
+        auto worker = make_infinite_coro_worker(
+                [=,
+                        cur_helper = VARIABLE_MANUAL_DELEGATE(transform_obj, cur_index)]() mutable
+                        -> boost::asio::awaitable<bool> {
+                    co_await cur_helper.coro_wait_update();
+                    auto ts = cur_helper.get_last_update_ts();
+                    auto offset = QUERY_VARIABLE(transform_obj, offset_index);
+                    if (cur_helper.empty() || offset == nullptr) [[unlikely]] {
+                        // do nothing
+                    } else [[likely]] {
+                        auto target = transform_obj::new_instance();
+                        target->value = cur_helper->value * offset->value;
+                        UPDATE_VARIABLE_WITH_TS(transform_obj, tar_index, target, ts);
+                    }
+                    co_return true;
+                });
+        return std::move(worker);
+    }
+
+}
+#endif //SOPHIAR2_SIMPLE_OFFSET_TO_POSE_CONTROLLER_HPP

src/core/global_defs.cpp

@@ -1,5 +1,8 @@
 #include "global_defs.h"
+#include "algorithm/five_dof_offset_calculator.hpp"
+#include "algorithm/tcp_offset_to_flange_calculator.hpp"
 #include "algorithm/transform_utility.hpp"
+#include "controller/simple_offset_to_pose_controller.hpp"
 #include "core/external_controller.h"
 #include "core/external_variable_io.h"
 #include "core/sophiar_manager.h"
@@ -79,6 +82,11 @@ namespace sophiar {
         REGISTER_TYPE(transform_tree);
         REGISTER_TYPE(transform_stabilizer);
         REGISTER_TYPE(landmark_registration);
+        // control utility
+        using five_dof_offset_calculator = simple_tristate_obj_wrapper<make_5dof_offset_calculator_func>;
+        using tcp_offset_to_flange_calculator = simple_tristate_obj_wrapper<make_tcp_offset_to_flange_calculator_func>;
+        REGISTER_TYPE(five_dof_offset_calculator);
+        REGISTER_TYPE(tcp_offset_to_flange_calculator);
         // transform utility
         using transform_inverter = simple_tristate_obj_wrapper<make_transform_inverter_func>;
         using scalarxyz_transformer = simple_tristate_obj_wrapper<make_scalarxyz_transformer>;
@@ -88,6 +96,12 @@ namespace sophiar {
 #endif
     }
 
+    void register_controllers() {
+        using simple_offset_to_pose_controller
+                = simple_tristate_obj_wrapper<make_simple_offset_to_pose_controller_func>;
+        REGISTER_TYPE(simple_offset_to_pose_controller);
+    }
+
     struct empty_object : public tristate_obj {
         DEFAULT_NEW_INSTANCE(empty_object);
     };
@@ -96,6 +110,7 @@ namespace sophiar {
 
         REGISTER_TYPE(empty_object);
         register_algorithms();
+        register_controllers();
 
 #if !SOPHIAR_TEST || SOPHIAR_TEST_ROBOT
 

src/main.cpp

@@ -6,11 +6,14 @@
 
 #include <nlohmann/json.hpp>
 
+#include <spdlog/spdlog.h>
+
 #include "utility/assert_utility.h"
 
 using namespace sophiar;
 
 int main(int argc, char *argv[]) {
+    spdlog::set_level(spdlog::level::trace);
     assert(argc == 2);
     std::ifstream config_file(argv[1]);
     assert(config_file.is_open());

src/utility/config_utility.hpp

@@ -53,6 +53,15 @@
         _val; \
     })
 
+#define TRY_LOAD_FLOAT_ITEM(var_name, default_val) ({ \
+    double _val = default_val; \
+    if (config.contains(var_name)) { \
+        assert(config[var_name].is_number()); \
+        _val = config[var_name].get<double>(); \
+    } \
+        _val; \
+    })
+
 #define TRY_LOAD_VARIABLE_INDEX(var_type, var_name) ({ \
     variable_index_type _index = -1; \
     if (config.contains(var_name)) { \

src/utility/variable_utility.hpp

@@ -32,12 +32,18 @@ namespace sophiar {
     inline coro_worker::pointer variable_debug_watcher_func(const nlohmann::json &config) {
         std::string var_name;
         auto var_index = LOAD_VARIABLE_INDEX_WITH_NAME(SmallObjType, "variable_name", var_name);
+        timestamp_type min_interval = TRY_LOAD_FLOAT_ITEM("minimum_interval_ms", 0) * 1000; // ms -> us
         auto worker = make_infinite_coro_worker(
                 [=,
                         buffer = string_writer(),
+                        last_print_ts = 0,
                         var_helper = VARIABLE_MANUAL_DELEGATE(SmallObjType, var_index)]() mutable
                         -> awaitable<bool> {
                     co_await var_helper.coro_wait_update();
+                    // prevent frequent print
+                    auto ts = var_helper.get_last_update_ts();
+                    if (ts - last_print_ts < min_interval) [[likely]] co_return true;
+                    last_print_ts = ts;
                     if (var_helper.empty()) {
                         SPDLOG_DEBUG("{} is empty.", var_name);
                     } else {
@@ -158,6 +164,7 @@ namespace sophiar {
                     auto input = QUERY_VARIABLE_WITH_TS(SmallObjType, input_index, &ts);
                     UPDATE_VARIABLE_WITH_TS(SmallObjType, output_index, input, ts);
                     UPDATE_VARIABLE_VAL(bool_obj, signal_index, false);
+                    signal.manual_sync();
                     co_return true;
                 });
         return std::move(worker);

tests/data/ndi_registration_config.json

@@ -127,14 +127,14 @@
     },
     {
       "type": "scalarxyz_transformer",
-      "name": "probe_tip_transformer",
+      "name": "probe_tip_transformer_for_model_ref",
       "start_config": {
         "transform_type": "point",
         "transform_var_name": "probe_in_model_ref",
         "target_value": [
-          -0.55,
-          -0.96,
-          1.72
+          0,
+          0,
+          0
         ],
         "output_var_name": "tip_in_model_ref"
       },
@@ -145,23 +145,23 @@
     },
     {
       "type": "transform_stabilizer",
-      "name": "point_picker",
+      "name": "point_picker_for_model_ref",
       "start_config": {
         "stable_type": "point",
         "input_var_name": "tip_in_model_ref",
         "output_var_name": "picked_point_in_model_ref",
-        "linear_tolerance_mm": 0.02,
+        "linear_tolerance_mm": 0.05,
         "angular_tolerance_deg": 0.01,
-        "temporal_interval_s": 1.5,
+        "temporal_interval_s": 3,
         "counting_interval": 150
       },
       "dependencies": [
-        "probe_tip_transformer"
+        "probe_tip_transformer_for_model_ref"
       ]
     },
     {
       "type": "scalarxyz_obj_watcher",
-      "name": "picked_point_watcher",
+      "name": "picked_point_watcher_for_model_ref",
       "start_config": {
         "variable_name": "picked_point_in_model_ref"
       }
@@ -172,24 +172,34 @@
       "start_config": {
         "fiducial_points": [
           [
-            0,
-            0,
-            0
+            185.75326529376449,
+            149.3433422806243,
+            37.89585779283788
           ],
           [
-            0,
-            50,
-            0
+            154.32787238465744,
+            116.4095837766868,
+            28.39577397806383
           ],
           [
-            50,
-            50,
-            0
+            137.3846660114195,
+            112.52950379923068,
+            27.82314813339255
           ],
           [
-            50,
-            0,
-            0
+            116.28380784689239,
+            149.60532673304985,
+            28.430980391981475
+          ],
+          [
+            151.4594107171369,
+            127.58239816414866,
+            33.605828902057866
+          ],
+          [
+            151.37168103784265,
+            131.5925312645366,
+            35.12088213541559
           ]
         ],
         "point_var_name": "picked_point_in_model_ref",
@@ -197,7 +207,7 @@
         "error_var_name": "model_in_model_ref_error"
       },
       "dependencies": [
-        "point_picker"
+        "point_picker_for_model_ref"
       ]
     },
     {
@@ -216,14 +226,14 @@
     },
     {
       "type": "scalarxyz_transformer",
-      "name": "probe_tip_transformer2",
+      "name": "probe_tip_transformer_for_model",
       "start_config": {
         "transform_type": "point",
         "transform_var_name": "probe_in_model",
         "target_value": [
-          -0.55,
-          -0.96,
-          1.72
+          0,
+          0,
+          0
         ],
         "output_var_name": "tip_in_model"
       },
@@ -234,34 +244,34 @@
     },
     {
       "type": "transform_stabilizer",
-      "name": "point_picker2",
+      "name": "point_picker_for_model",
       "start_config": {
         "stable_type": "point",
         "input_var_name": "tip_in_model",
         "output_var_name": "picked_point_in_model",
-        "linear_tolerance_mm": 0.02,
+        "linear_tolerance_mm": 0.05,
         "angular_tolerance_deg": 0.01,
-        "temporal_interval_s": 1.5,
+        "temporal_interval_s": 3,
         "counting_interval": 150
       },
       "dependencies": [
-        "probe_tip_transformer2"
+        "probe_tip_transformer_for_model"
       ]
     },
     {
       "type": "scalarxyz_obj_watcher",
-      "name": "picked_point_watcher2",
+      "name": "picked_point_watcher_for_model",
       "start_config": {
         "variable_name": "picked_point_in_model"
       }
     },
     {
       "type": "empty_object",
-      "name": "all",
+      "name": "reg_model",
       "dependencies": [
         "probe_visibility_watcher",
         "model_ref_visibility_watcher",
-        "picked_point_watcher",
+        "picked_point_watcher_for_model_ref",
         "registration_result_watcher",
         "registration_error_watcher",
         "model_registrator"
@@ -269,12 +279,13 @@
     },
     {
       "type": "empty_object",
-      "name": "all2",
+      "name": "icp_model",
       "dependencies": [
         "probe_visibility_watcher",
         "model_ref_visibility_watcher",
-        "picked_point_watcher2",
-        "point_picker2"
+        "picked_point_watcher_for_model",
+        "point_picker_for_model",
+        "registration_result_watcher"
       ]
     }
   ]

tests/data/ndi_registration_config_kuka.json

@@ -0,0 +1,286 @@
+{
+  "controller_port": 5277,
+  "variable_list": [
+    {
+      "name": "probe_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "model_ref_in_tracker",
+      "type": "transform_obj"
+    },
+    {
+      "name": "model_in_model_ref",
+      "type": "transform_obj",
+      "value": [
+        135.945,
+        39.1189,
+        -267.07,
+        0.536927,
+        0.60565,
+        -0.471815,
+        -0.349698
+      ]
+    },
+    {
+      "name": "probe_in_model_ref",
+      "type": "transform_obj"
+    },
+    {
+      "name": "probe_in_model",
+      "type": "transform_obj"
+    },
+    {
+      "name": "tip_in_model_ref",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "picked_point_in_model_ref",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "model_in_model_ref_error",
+      "type": "double_obj"
+    },
+    {
+      "name": "tip_in_model",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "picked_point_in_model",
+      "type": "scalarxyz_obj"
+    }
+  ],
+  "object_list": [
+    {
+      "type": "transform_tree",
+      "name": "transform_tree",
+      "init_config": {
+        "node_list": [
+          {
+            "name": "tracker"
+          },
+          {
+            "name": "probe",
+            "parent": "tracker",
+            "transform_var_name": "probe_in_tracker"
+          },
+          {
+            "name": "model_ref",
+            "parent": "tracker",
+            "transform_var_name": "model_ref_in_tracker"
+          },
+          {
+            "name": "model",
+            "parent": "model_ref",
+            "transform_var_name": "model_in_model_ref"
+          }
+        ]
+      },
+      "start_config": {
+        "watch_list": [
+          {
+            "target": "probe",
+            "observer": "model_ref",
+            "transform_var_name": "probe_in_model_ref"
+          },
+          {
+            "target": "probe",
+            "observer": "model",
+            "transform_var_name": "probe_in_model"
+          }
+        ]
+      }
+    },
+    {
+      "type": "ndi_interface",
+      "name": "ndi",
+      "init_config": {
+        "address_type": "ethernet",
+        "ip": "10.0.0.5",
+        "tcp_port": 8765,
+        "com_port": "COM3",
+        "tool_list": [
+          {
+            "rom_path": "/home/tpx/data/roms/Drill_3Ball.rom",
+            "outputs": {
+              "transform": "model_ref_in_tracker"
+            }
+          },
+          {
+            "rom_path": "/home/tpx/data/roms/Probe_Small_3Ball.rom",
+            "outputs": {
+              "transform": "probe_in_tracker"
+            }
+          }
+        ]
+      },
+      "start_config": {
+        "allow_unreliable": false,
+        "prefer_stream_tracking": false
+      }
+    },
+    {
+      "type": "transform_obj_validity_watcher",
+      "name": "probe_visibility_watcher",
+      "start_config": {
+        "variable_name": "probe_in_tracker"
+      }
+    },
+    {
+      "type": "transform_obj_validity_watcher",
+      "name": "model_ref_visibility_watcher",
+      "start_config": {
+        "variable_name": "model_ref_in_tracker"
+      }
+    },
+    {
+      "type": "scalarxyz_transformer",
+      "name": "probe_tip_transformer_for_model_ref",
+      "start_config": {
+        "transform_type": "point",
+        "transform_var_name": "probe_in_model_ref",
+        "target_value": [
+          0,
+          0,
+          0
+        ],
+        "output_var_name": "tip_in_model_ref"
+      },
+      "dependencies": [
+        "ndi",
+        "transform_tree"
+      ]
+    },
+    {
+      "type": "transform_stabilizer",
+      "name": "point_picker_for_model_ref",
+      "start_config": {
+        "stable_type": "point",
+        "input_var_name": "tip_in_model_ref",
+        "output_var_name": "picked_point_in_model_ref",
+        "linear_tolerance_mm": 0.05,
+        "angular_tolerance_deg": 0.01,
+        "temporal_interval_s": 3,
+        "counting_interval": 150
+      },
+      "dependencies": [
+        "probe_tip_transformer_for_model_ref"
+      ]
+    },
+    {
+      "type": "scalarxyz_obj_watcher",
+      "name": "picked_point_watcher_for_model_ref",
+      "start_config": {
+        "variable_name": "picked_point_in_model_ref"
+      }
+    },
+    {
+      "type": "landmark_registration",
+      "name": "model_registrator",
+      "start_config": {
+        "fiducial_points": [
+          [
+            9.16363,
+            245.77,
+            -35.5363
+          ],
+          [
+            -38.2517,
+            238.265,
+            15.8886
+          ],
+          [
+            -18.4721,
+            228.506,
+            -24.2903
+          ]
+        ],
+        "point_var_name": "picked_point_in_model_ref",
+        "transform_var_name": "model_in_model_ref",
+        "error_var_name": "model_in_model_ref_error"
+      },
+      "dependencies": [
+        "point_picker_for_model_ref"
+      ]
+    },
+    {
+      "type": "transform_obj_watcher",
+      "name": "registration_result_watcher",
+      "start_config": {
+        "variable_name": "model_in_model_ref"
+      }
+    },
+    {
+      "type": "double_obj_watcher",
+      "name": "registration_error_watcher",
+      "start_config": {
+        "variable_name": "model_in_model_ref_error"
+      }
+    },
+    {
+      "type": "scalarxyz_transformer",
+      "name": "probe_tip_transformer_for_model",
+      "start_config": {
+        "transform_type": "point",
+        "transform_var_name": "probe_in_model",
+        "target_value": [
+          0,
+          0,
+          0
+        ],
+        "output_var_name": "tip_in_model"
+      },
+      "dependencies": [
+        "ndi",
+        "transform_tree"
+      ]
+    },
+    {
+      "type": "transform_stabilizer",
+      "name": "point_picker_for_model",
+      "start_config": {
+        "stable_type": "point",
+        "input_var_name": "tip_in_model",
+        "output_var_name": "picked_point_in_model",
+        "linear_tolerance_mm": 0.05,
+        "angular_tolerance_deg": 0.01,
+        "temporal_interval_s": 3,
+        "counting_interval": 150
+      },
+      "dependencies": [
+        "probe_tip_transformer_for_model"
+      ]
+    },
+    {
+      "type": "scalarxyz_obj_watcher",
+      "name": "picked_point_watcher_for_model",
+      "start_config": {
+        "variable_name": "picked_point_in_model"
+      }
+    },
+    {
+      "type": "empty_object",
+      "name": "reg_model",
+      "dependencies": [
+        "probe_visibility_watcher",
+        "model_ref_visibility_watcher",
+        "picked_point_watcher_for_model_ref",
+        "registration_result_watcher",
+        "registration_error_watcher",
+        "model_registrator"
+      ]
+    },
+    {
+      "type": "empty_object",
+      "name": "icp_model",
+      "dependencies": [
+        "probe_visibility_watcher",
+        "model_ref_visibility_watcher",
+        "picked_point_watcher_for_model",
+        "point_picker_for_model",
+        "registration_result_watcher"
+      ]
+    }
+  ]
+}

tests/tracker/ndi_registration.cpp

@@ -13,7 +13,7 @@ using namespace std::chrono_literals;
 
 BOOST_AUTO_TEST_CASE(test_ndi_registration) {
     spdlog::set_level(spdlog::level::trace);
-    std::ifstream config_file("data/ndi_registration_config.json");
+    std::ifstream config_file("data/ndi_registration_config_kuka.json");
     BOOST_TEST(config_file.is_open());
     auto config = nlohmann::json::parse(config_file);
     auto ok = initialize(config);