Fixed some problem.

app/data/kuka_navigation.json

@@ -13,13 +13,13 @@
       "name": "model_in_model_ref",
       "type": "transform_obj",
       "value": [
-        135.945,
-        39.1189,
-        -267.07,
-        0.536927,
-        0.60565,
-        -0.471815,
-        -0.349698
+        134.705,
+        42.6256,
+        -267.533,
+        0.535693,
+        0.610751,
+        -0.464744,
+        -0.352178
       ]
     },
     {
@@ -31,20 +31,11 @@
       "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": "tool_in_model",
+      "name": "tool_in_robot_flange",
       "type": "transform_obj"
     },
     {
@@ -150,13 +141,13 @@
             "name": "robot_base",
             "parent": "tracker",
             "transform": [
-              249.608683668030,
-              378.363630568367,
-              -1999.86036620645,
-              0.694408974678311,
-              0.320622130095823,
-              -0.539876883852255,
-              -0.351469167724585
+              227.964774380901,
+              408.415090613534,
+              -2083.90135385252,
+              0.718310380441512,
+              0.277543700520970,
+              -0.563300905075462,
+              -0.299485862758597
             ]
           },
           {
@@ -167,15 +158,23 @@
           {
             "name": "tool_real",
             "parent": "robot_flange",
-            "transform_var_name": "tool_in_flange_const"
+            "transform": [
+              193.674181012287,
+              119.007940358412,
+              0.975841097441376,
+              0.340960180366781,
+              -0.343553889200713,
+              -0.619099078363195,
+              0.618411846417222
+            ]
           },
           {
             "name": "tool",
             "parent": "tool_real",
             "transform": [
-              0,
-              0,
-              -30,
+              6,
+              5,
+              -25,
               1,
               0,
               0,
@@ -200,6 +199,11 @@
             "target": "tool",
             "observer": "model",
             "transform_var_name": "tool_in_model"
+          },
+          {
+            "target": "tool",
+            "observer": "robot_flange",
+            "transform_var_name": "tool_in_robot_flange"
           }
         ]
       }
@@ -409,7 +413,7 @@
       "name": "flange_offset_calculator",
       "start_config": {
         "tcp_offset_transform_var": "target_tool_offset",
-        "tcp_in_flange_transform_var": "tool_in_flange_const",
+        "tcp_in_flange_transform_var": "tool_in_robot_flange",
         "flange_offset_transform_var": "target_flange_offset"
       },
       "dependencies": [

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.3;
+    static constexpr auto move_angle = 0.35;
     static constexpr auto move_distance = 75;
 
     static constexpr auto pi = std::numbers::pi;

src/algorithm/five_dof_offset_calculator.hpp

@@ -36,15 +36,13 @@ namespace sophiar {
                     if (cur_trans == nullptr || tar_trans == nullptr) [[unlikely]] {
                         UPDATE_VARIABLE_WITH_TS(transform_obj, offset_index, nullptr, ts);
                     } else [[likely]] {
+                        auto dof6_trans = cur_trans->value.inverse() * tar_trans->value;
                         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 new_z_axis = dof6_trans.linear() * z_axis;
+                        auto dof5_rot = Eigen::Quaterniond::FromTwoVectors(z_axis, new_z_axis);
+                        auto dof5_trans = Eigen::Translation3d{dof6_trans.translation()};
                         auto offset = transform_obj::new_instance();
-                        offset->value = trans_part * rot_part;
-                        offset->value = cur_trans->value.inverse() * offset->value * cur_trans->value;
+                        offset->value = dof5_trans * dof5_rot;
                         UPDATE_VARIABLE_WITH_TS(transform_obj, offset_index, offset, ts);
                     }
                     co_return true;

src/algorithm/transform_tree.cpp

@@ -103,14 +103,18 @@ namespace sophiar {
             auto exit_func = SIGNAL_GROUP_AUTO_CLOSER(signal_group);
 
             auto worker_func = [=, this,
+                    first_run = true,
                     scheme = std::move(scheme),
                     signal_watcher = std::move(signal_watcher)]() mutable
                     -> boost::asio::awaitable<bool> {
-                co_await signal_watcher.coro_wait();
+                if (!first_run) { // set initial value for watched transform
+                    co_await signal_watcher.coro_wait();
+                }
                 auto new_trans = get_transform_from_scheme(scheme);
                 auto new_trans_obj = new_trans.has_value() ?
                                      transform_obj::new_instance(std::move(new_trans.value())) : nullptr;
                 UPDATE_VARIABLE(transform_obj, out_var_index, new_trans_obj);
+                first_run = false;
                 co_return true;
             };
             auto worker = make_infinite_coro_worker(std::move(worker_func),

src/utility/debug_utility.hpp

@@ -3,6 +3,7 @@
 
 #include "core/timestamp_helper.hpp"
 #include "third_party/scope_guard.hpp"
+#include "utility/versatile_buffer2.hpp"
 
 #include <boost/asio/awaitable.hpp>
 #include <boost/asio/high_resolution_timer.hpp>
@@ -79,6 +80,15 @@ namespace sophiar {
         co_return;
     }
 
+    template<typename BasicObjType>
+    void print_variable(const char *var_name, const BasicObjType &var) {
+        auto ss = string_writer{};
+        var->write_to(ss);
+        std::cout << fmt::format("{} = {}", var_name, ss.get_string_and_reset()) << std::endl;
+    }
+
+#define PRINT_VARIABLE(var) print_variable(#var, var)
+
 }
 
 #endif //SOPHIAR2_DEBUG_MACRO_H