Implemented the data collection for kuka calibration.

CMakeLists.txt

@@ -32,7 +32,7 @@ file(GLOB_RECURSE CORE_IMPL_FILES ./src/core/*.cpp)
 file(GLOB_RECURSE ROBOT_IMPL_FILES ./src/robot/*.cpp)
 
 file(GLOB_RECURSE SRC_FILES ./src/*.cpp)
-add_executable(${PROJECT_NAME} ${SRC_FILES} src/robot/kuka/kuka_interface.cpp src/utility/coro_unique_lock.hpp src/robot/ur/ur_defs.h src/robot/kuka/kuka_defs.h)
+add_executable(${PROJECT_NAME} ${SRC_FILES})
 
 IF (WIN32)
     list(APPEND EXTRA_LIBS ws2_32 wsock32 winmm bcrypt)
@@ -42,5 +42,6 @@ ENDIF ()
 
 target_link_libraries(${PROJECT_NAME} ${EXTRA_LIBS})
 
+add_subdirectory(app)
 add_subdirectory(benchmark)
 add_subdirectory(tests)

app/CMakeLists.txt

@@ -0,0 +1,5 @@
+list(FILTER SRC_FILES EXCLUDE REGEX "main.cpp$") # remove global main
+add_executable(kuka_calibration
+        kuka_calibration.cpp
+        ${SRC_FILES})
+target_link_libraries(kuka_calibration ${EXTRA_LIBS})

app/data/kuka_calibration_config.json

@@ -0,0 +1,98 @@
+{
+  "controller_port": 5277,
+  "variable_list": [
+    {
+      "name": "flange_in_base",
+      "type": "transform_obj"
+    },
+    {
+      "name": "robot_is_moving",
+      "type": "bool_obj"
+    },
+    {
+      "name": "kuka_control_mode",
+      "type": "u64int_obj"
+    },
+    {
+      "name": "target_flange_pose",
+      "type": "transform_obj"
+    },
+    {
+      "name": "robot_flange_speed_limit",
+      "type": "double_obj",
+      "value": 10
+    },
+    {
+      "name": "kuka_joint_speed_limit",
+      "type": "array7_obj",
+      "value": [
+        0.1,
+        0.1,
+        0.1,
+        0.1,
+        0.1,
+        0.1,
+        0.1
+      ]
+    },
+    {
+      "name": "ref_in_tracker",
+      "type": "transform_obj"
+    }
+  ],
+  "object_list": [
+    {
+      "type": "kuka_interface",
+      "name": "kuka",
+      "init_config": {
+        "address": "10.0.0.10"
+      },
+      "start_config": {
+        "output_config": {
+          "report_frequency": 50,
+          "flange_info": {
+            "pose": "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": "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": "ref_in_tracker"
+            }
+          }
+        ]
+      },
+      "start_config": {
+        "allow_unreliable": true,
+        "prefer_stream_tracking": false
+      }
+    },
+    {
+      "type": "empty_object",
+      "name": "all",
+      "dependencies": [
+        "kuka",
+        "ndi"
+      ]
+    }
+  ]
+}

app/kuka_calibration.cpp

@@ -0,0 +1,85 @@
+#include "core/basic_obj_types.hpp"
+#include "core/global_defs.h"
+#include "core/sophiar_pool.h"
+#include "utility/debug_utility.hpp"
+#include "utility/variable_helper.hpp"
+#include "utility/versatile_buffer2.hpp"
+
+#include <boost/asio/co_spawn.hpp>
+#include <boost/asio/detached.hpp>
+
+#include <spdlog/spdlog.h>
+
+#include <fstream>
+#include <iostream>
+#include <numbers>
+
+#include "utility/assert_utility.h"
+
+using namespace Eigen;
+using namespace sophiar;
+using namespace std::chrono_literals;
+
+using boost::asio::co_spawn;
+using boost::asio::awaitable;
+using boost::asio::detached;
+
+awaitable<void> collect_data() {
+    static constexpr auto resolution = 32;
+    static constexpr auto move_angle = 0.5;
+    static constexpr auto move_distance = 75;
+
+    static constexpr auto pi = std::numbers::pi;
+    static constexpr auto step_angle = 2 * pi / resolution;
+
+    auto flange_index = REQUIRE_VARIABLE(transform_obj, "flange_in_base");
+    auto ref_index = REQUIRE_VARIABLE(transform_obj, "ref_in_tracker");
+    auto is_moving_index = REQUIRE_VARIABLE(bool_obj, "robot_is_moving");
+    auto target_index = REQUIRE_VARIABLE(transform_obj, "target_flange_pose");
+
+    auto print_data = [=, ss = string_writer{}]() mutable {
+        QUERY_VARIABLE(transform_obj, flange_index)->write_to(ss);
+        std::cout << ss.get_string_and_reset() << std::endl;
+        QUERY_VARIABLE(transform_obj, ref_index)->write_to(ss);
+        std::cout << ss.get_string_and_reset() << std::endl;
+    };
+
+    // wait for robot ready
+    co_await coro_wait_for_not_empty<transform_obj>(flange_index);
+    co_await coro_wait_for_not_empty<transform_obj>(ref_index);
+
+    // print initial states
+    print_data();
+
+    // start moving
+    auto start_pose = QUERY_VARIABLE(transform_obj, flange_index);
+    for (int i = 0; i < resolution; ++i) {
+        auto angle = i * step_angle;
+        auto rot_axis = Vector3d{cos(angle), sin(angle), 0};
+        auto next_pose = transform_obj::new_instance();
+        next_pose->value = start_pose->value
+                           * Translation3d{move_distance * rot_axis}
+                           * AngleAxisd{move_angle, rot_axis};
+        UPDATE_VARIABLE(transform_obj, target_index, next_pose);
+        // wait for movement to start and then finish
+        co_await coro_wait_for_variable_value<bool_obj>(is_moving_index, true);
+        co_await coro_wait_for_variable_value<bool_obj>(is_moving_index, false);
+        // wait for robot to be stable
+        co_await coro_sleep(1s);
+        print_data();
+    }
+
+    // move back to start pose
+    UPDATE_VARIABLE(transform_obj, target_index, start_pose);
+    co_return;
+}
+
+int main() {
+    spdlog::set_level(spdlog::level::trace);
+    std::ifstream config_file("data/kuka_calibration_config.json");
+    assert(config_file.is_open());
+    auto config = nlohmann::json::parse(config_file);
+    assert(initialize(config));
+    co_spawn(*global_context, collect_data(), detached);
+    global_context->run();
+}

src/core/global_defs.cpp

@@ -58,7 +58,9 @@ namespace sophiar {
         using var_type##_recorder = variable_recorder<var_type>; \
         REGISTER_TYPE(var_type##_recorder); \
         using var_type##_replayer = variable_replayer<var_type>; \
-        REGISTER_TYPE(var_type##_replayer);
+        REGISTER_TYPE(var_type##_replayer); \
+        using var_type##_d_latch = variable_d_latch<var_type>; \
+        REGISTER_TYPE(var_type##_d_latch);
 
         REGISTER_VARIABLE_TYPE(bool_obj);
         REGISTER_VARIABLE_TYPE(u64int_obj);

src/utility/debug_utility.hpp

@@ -57,7 +57,6 @@
     { \
         bool ok = co_await (func); \
         if (!ok) { \
-            assert(false);  \
             co_return false; \
         } \
     }

src/utility/variable_utility.hpp

@@ -143,6 +143,26 @@ namespace sophiar {
         return std::move(worker);
     }
 
+    template<typename SmallObjType>
+    inline coro_worker::pointer variable_d_latch_func(const nlohmann::json &config) {
+        auto input_index = LOAD_VARIABLE_INDEX(SmallObjType, "input_name");
+        auto output_index = LOAD_VARIABLE_INDEX(SmallObjType, "output_name");
+        auto signal_index = LOAD_VARIABLE_INDEX(bool_obj, "signal_name");
+        // create worker
+        auto worker = make_infinite_coro_worker(
+                [=, signal = VARIABLE_MANUAL_DELEGATE(bool_obj, signal_index)]() mutable
+                        -> awaitable<bool> {
+                    co_await signal.coro_wait_update();
+                    assert(signal->value == true);
+                    timestamp_type ts;
+                    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);
+                    co_return true;
+                });
+        return std::move(worker);
+    }
+
     template<typename SmallObjType>
     using variable_debug_watcher = simple_tristate_obj_wrapper<variable_debug_watcher_func<SmallObjType>>;
 
@@ -155,6 +175,9 @@ namespace sophiar {
     template<typename SmallObjType>
     using variable_replayer = simple_tristate_obj_wrapper<variable_replayer_func<SmallObjType>>;
 
+    template<typename SmallObjType>
+    using variable_d_latch = simple_tristate_obj_wrapper<variable_d_latch_func<SmallObjType>>;
+
 }
 
 #endif //SOPHIAR2_VARIABLE_UTILITY_HPP

tests/data/kuka_interface_config.json

@@ -47,7 +47,8 @@
     },
     {
       "name": "robot_flange_speed_limit",
-      "type": "double_obj"
+      "type": "double_obj",
+      "value": 10
     },
     {
       "name": "robot_flange_acceleration_limit",
@@ -59,7 +60,16 @@
     },
     {
       "name": "kuka_joint_speed_limit",
-      "type": "array7_obj"
+      "type": "array7_obj",
+      "value": [
+        0.1,
+        0.1,
+        0.1,
+        0.1,
+        0.1,
+        0.1,
+        0.1
+      ]
     },
     {
       "name": "kuka_joint_acceleration_limit",

tests/robot/kuka_interface.cpp

@@ -76,12 +76,55 @@ awaitable<void> test_kuka_flange_pose_control() {
     co_return;
 }
 
+awaitable<void> test_kuka_base_calibration() {
+    auto flange_pose_index = REQUIRE_VARIABLE(transform_obj, "robot_flange_pose");
+    auto target_index = REQUIRE_VARIABLE(transform_obj, "target_flange_pose");
+    auto is_moving_index = REQUIRE_VARIABLE(bool_obj, "robot_is_moving");
+    co_await coro_wait_for_not_empty<transform_obj>(flange_pose_index);
+    auto old_flange_pose = QUERY_VARIABLE(transform_obj, flange_pose_index);
+
+    co_await coro_sleep(5s);
+
+    // 1st pose
+    auto next_flange_pose = transform_obj::new_instance();
+    next_flange_pose->value = old_flange_pose->value * Eigen::AngleAxisd(0.4, Eigen::Vector3d::UnitY());
+    UPDATE_VARIABLE(transform_obj, target_index, next_flange_pose);
+    co_await coro_wait_for_variable_value<bool_obj>(is_moving_index, true);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, is_moving_index)->value == true);
+    co_await coro_wait_for_variable_value<bool_obj>(is_moving_index, false);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, is_moving_index)->value == false);
+
+    co_await coro_sleep(1s);
+
+    // 2st pose
+    next_flange_pose = transform_obj::new_instance();
+    next_flange_pose->value = old_flange_pose->value * Eigen::AngleAxisd(-0.4, Eigen::Vector3d::UnitY());
+    UPDATE_VARIABLE(transform_obj, target_index, next_flange_pose);
+    co_await coro_wait_for_variable_value<bool_obj>(is_moving_index, true);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, is_moving_index)->value == true);
+    co_await coro_wait_for_variable_value<bool_obj>(is_moving_index, false);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, is_moving_index)->value == false);
+
+    co_await coro_sleep(1s);
+
+    // old pose
+    next_flange_pose = transform_obj::new_instance();
+    next_flange_pose->value = old_flange_pose->value;
+    UPDATE_VARIABLE(transform_obj, target_index, next_flange_pose);
+    co_await coro_wait_for_variable_value<bool_obj>(is_moving_index, true);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, is_moving_index)->value == true);
+    co_await coro_wait_for_variable_value<bool_obj>(is_moving_index, false);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, is_moving_index)->value == false);
+
+    co_return;
+}
+
 BOOST_AUTO_TEST_CASE(test_kuka_interface) {
     spdlog::set_level(spdlog::level::trace);
     std::ifstream config_file("data/kuka_interface_config.json");
     BOOST_TEST(config_file.is_open());
     auto config = nlohmann::json::parse(config_file);
     BOOST_TEST(initialize(config));
-    co_spawn(*global_context, test_kuka_flange_pose_control(), detached);
+    co_spawn(*global_context, test_kuka_base_calibration(), detached);
     global_context->run();
 }