Ready to test fracture robot.

CMakeLists.txt

@@ -1,10 +1,8 @@
 cmake_minimum_required(VERSION 3.0)
 
-set(PROJECT_NAME Sophiar2)
+project(Sophiar2)
 set(CMAKE_CXX_STANDARD 20)
 
-project(${PROJECT_NAME})
-
 add_compile_options(-march=native)
 add_compile_options(-mno-avx) # enable avx will cause some stack pointer alignment error with Eigen
 

src/core/global_defs.cpp

@@ -21,6 +21,7 @@
 
 DEFAULT_TRISTATE_OBJ_DEF(ur_interface)
 DEFAULT_TRISTATE_OBJ_DEF(kuka_interface)
+DEFAULT_TRISTATE_OBJ_DEF(fracture_robot_interface)
 
 #endif
 
@@ -100,6 +101,7 @@ namespace sophiar {
 
         REGISTER_TYPE(ur_interface);
         REGISTER_TYPE(kuka_interface);
+        REGISTER_TYPE(fracture_robot_interface);
 
 #endif
 

src/robot/fracture_robot/fracture_robot_defs.h

@@ -0,0 +1,15 @@
+#ifndef SOPHIAR2_FRACTURE_ROBOT_DEFS_H
+#define SOPHIAR2_FRACTURE_ROBOT_DEFS_H
+
+#include <cstdint>
+
+namespace sophiar {
+
+    enum class fracture_robot_control_mode_type : uint8_t {
+        DIRECT = 1,
+        CARTESIAN = 2,
+    };
+
+}
+
+#endif //SOPHIAR2_FRACTURE_ROBOT_DEFS_H

src/robot/fracture_robot/fracture_robot_interface.cpp

@@ -0,0 +1,361 @@
+#include "core/basic_obj_types.hpp"
+#include "robot/fracture_robot/fracture_robot_defs.h"
+#include "utility/config_utility.hpp"
+#include "utility/coro_worker.hpp"
+#include "utility/coro_worker_helper_func.hpp"
+#include "utility/name_translator.hpp"
+#include "utility/singleton_helper.hpp"
+#include "utility/variable_helper.hpp"
+#include "utility/versatile_buffer2.hpp"
+
+#include <boost/asio/serial_port.hpp>
+#include <boost/asio/use_awaitable.hpp>
+#include <boost/smart_ptr/scoped_ptr.hpp>
+
+#include <spdlog/spdlog.h>
+
+#include "utility/assert_utility.h"
+
+DEFAULT_TRISTATE_OBJ_DEF(fracture_robot_interface)
+
+namespace sophiar {
+
+    using boost::asio::async_read;
+    using boost::asio::async_write;
+    using boost::asio::awaitable;
+    using boost::asio::serial_port;
+    using boost::asio::use_awaitable;
+    using boost::scoped_ptr;
+
+    struct fracture_robot_interface::impl {
+
+        using control_value_type = std::array<int32_t, 6>;
+        using control_mode_type = fracture_robot_control_mode_type;
+
+        struct control_mode_translator_type :
+                public singleton_helper<control_mode_translator_type>,
+                public name_translator<control_mode_type> {
+            void initialize() override {
+                register_item("direct", control_mode_type::DIRECT);
+                register_item("cartesian", control_mode_type::CARTESIAN);
+            }
+        };
+
+        inline static auto control_mode_translator
+                = control_mode_translator_type::get_instance();
+
+        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 = 4 * sizeof(int32_t) + 2;
+        static constexpr uint16_t control_packet_tail = 0x7fff;
+
+        struct screw_info {
+            Eigen::Vector3d origin; // Point on the screw
+            Eigen::Vector3d direction; // Direction of the screw
+        };
+
+        fracture_robot_interface *q_this = nullptr;
+
+        screw_info top_screw, side_screw;
+        double max_velocity = default_max_velocity;
+        double controller_gain = default_controller_gain;
+        control_mode_type current_control_mode;
+
+        variable_index_type current_control_mode_index = -1;
+        variable_index_type is_moving_index = -1;
+        // direct control
+        variable_index_type target_motor_speed_index = -1;
+        // cartesian control
+        variable_index_type current_tcp_pose_index = -1, target_tcp_pose_index = -1;
+
+        scoped_ptr<serial_port> conn;
+        coro_worker::pointer control_worker;
+
+        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;
+
+            { // side cross
+                auto origin = trans * side_screw.origin;
+                auto direction = trans.linear() * side_screw.direction;
+                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
+            }
+            { // top cross
+                auto origin = trans * top_screw.origin;
+                auto direction = trans.linear() * top_screw.direction;
+                auto t1 = (top1_y_offset - origin.y()) / direction.y();
+                auto t2 = (top2_y_offset - origin.y()) / direction.y();
+                auto cross1 = origin + t1 * direction;
+                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
+            }
+        }
+
+        static void encode_arr(const array6_obj::value_type &vec_val,
+                               control_value_type &arr_val) {
+            static constexpr int base_freq = 100000; // Hz
+            static constexpr auto dis_per_clock = 0.006; // mm
+            static constexpr auto minimum_velocity = 0.05; // mm/s, lower than this will cause stop
+
+            for (int i = 0; i < 6; ++i) {
+                if (std::abs(vec_val[i]) < minimum_velocity) {
+                    arr_val[i] = 0; // stop
+                } else {
+                    auto tar_freq = vec_val[i] / dis_per_clock;
+                    arr_val[i] = static_cast<int32_t>(base_freq / tar_freq);
+                    assert(std::abs(arr_val[i]) < 0x7fff);
+                    if ((arr_val[i] & 0xff) == 0x7f) [[unlikely]] { // make the LSB not 0x7f
+                        ++arr_val[i];
+                    }
+                }
+            }
+        }
+
+        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
+
+            double cur_max_vec = 0;
+            for (int i = 0; i < 6; ++i) {
+                auto dis_offset = tar_pos[i] - cur_pos[i];
+                if (std::abs(dis_offset) < dis_tolerance) {
+                    vec_val[i] = 0; // stop
+                } else {
+                    vec_val[i] = controller_gain * dis_offset;
+                    cur_max_vec = std::max(std::abs(vec_val[i]), cur_max_vec);
+                }
+            }
+
+            // clamp velocity
+            if (cur_max_vec > max_velocity) {
+                auto shrink_factor = max_velocity / cur_max_vec;
+                for (int i = 0; i < 6; ++i) {
+                    vec_val[i] *= shrink_factor;
+                }
+            }
+        }
+
+        awaitable<void> send_control_packet(const array6_obj::value_type &vec_val) {
+            // encode packet
+            auto arr_val = control_value_type{};
+            encode_arr(vec_val, arr_val);
+
+            // write packet
+            bool is_moving = false;
+            auto mem = static_memory<control_packet_length>{};
+            auto writer = versatile_writer<robot_endian>(mem);
+            for (auto item: arr_val) {
+                writer << item;
+                if (item != 0) {
+                    is_moving = true;
+                }
+            }
+            writer << control_packet_tail;
+
+            // send packet
+            assert(conn->is_open());
+            assert(writer.remaining_bytes() == 0);
+            co_await async_write_memory_to(*conn, mem);
+
+            // update moving status
+            try_update_variable_value<bool_obj>(is_moving_index, is_moving);
+            co_return;
+        }
+
+        auto create_direct_control_func() {
+            assert(current_control_mode == control_mode_type::DIRECT);
+            assert(is_valid_id(target_motor_speed_index));
+            auto worker_func = [=, this,
+                    motor_speed = VARIABLE_MANUAL_DELEGATE(array6_obj, target_motor_speed_index)]() mutable
+                    -> awaitable<bool> {
+                co_await motor_speed.coro_wait_update();
+                if (motor_speed.empty()) co_return true;
+                co_await send_control_packet(motor_speed->value);
+                co_return true;
+            };
+            return std::move(worker_func);
+        };
+
+        auto create_cartesian_control_func() {
+            assert(current_control_mode == control_mode_type::CARTESIAN);
+            assert(is_valid_id(current_tcp_pose_index));
+            assert(is_valid_id(target_tcp_pose_index));
+            auto worker_func = [=, this,
+                    current_tcp = VARIABLE_MANUAL_DELEGATE(transform_obj, current_tcp_pose_index),
+                    target_tcp = VARIABLE_MANUAL_DELEGATE(transform_obj, target_tcp_pose_index),
+                    target_pos = array6_obj::value_type{}]() mutable
+                    -> awaitable<bool> {
+                co_await current_tcp.coro_wait_update();
+                if (current_tcp.empty()) co_return true;
+                if (target_tcp.manual_sync()) { // target pose has been updated
+                    fake_inverse_kinematics(target_tcp->value, target_pos);
+                }
+                auto current_pos = array6_obj::value_type{};
+                fake_inverse_kinematics(current_tcp->value, current_pos);
+                auto vec_val = array6_obj::value_type{};
+                calc_target_velocity(current_pos, target_pos, vec_val);
+                co_await send_control_packet(vec_val);
+                co_return true;
+            };
+            return std::move(worker_func);
+        };
+
+        void create_control_worker() {
+            assert(control_worker == nullptr);
+
+            auto exception_handle_func = [](std::exception &) {
+                // TODO show log
+            };
+            auto exit_func = reset_on_exit_func(q_this);
+
+            switch (current_control_mode) {
+                case control_mode_type::DIRECT: {
+                    auto noexcept_wrapper = make_noexcept_func(
+                            create_direct_control_func(), std::move(exception_handle_func));
+                    control_worker = make_infinite_coro_worker(std::move(noexcept_wrapper),
+                                                               std::move(exit_func));
+                    break;
+                }
+                case control_mode_type::CARTESIAN: {
+                    auto noexcept_wrapper = make_noexcept_func(
+                            create_cartesian_control_func(), std::move(exception_handle_func));
+                    control_worker = make_infinite_coro_worker(std::move(noexcept_wrapper),
+                                                               std::move(exit_func));
+                    break;
+                }
+                default: {
+                    assert(false);
+                    return;
+                }
+            }
+
+            control_worker->run();
+
+            // set initial state
+            try_update_variable_value<bool_obj>(is_moving_index, false);
+        }
+
+        void setup_connection(const nlohmann::json &config) {
+            assert(conn == nullptr);
+            conn.reset(new serial_port(*global_context));
+
+            // open serial port
+            auto com_port_name = LOAD_STRING_ITEM("com_port");
+            conn->open(com_port_name);
+            assert(conn->is_open());
+
+            // config
+            conn->set_option(serial_port::baud_rate(9600));
+            conn->set_option(serial_port::stop_bits(serial_port::stop_bits::one));
+            conn->set_option(serial_port::parity(serial_port::parity::none));
+            conn->set_option(serial_port::character_size(8));
+            conn->set_option(serial_port::flow_control(serial_port::flow_control::none));
+        }
+
+        void load_init_config(const nlohmann::json &config) {
+            assert(config.contains("screw"));
+            auto &screw_config = config["screw"];
+            assert(screw_config.contains("top"));
+            { // top screw
+                auto &top_screw_config = screw_config["top"];
+                assert(top_screw_config.contains("origin"));
+                top_screw.origin = scalarxyz_obj::read_value_from_json_array(top_screw_config["origin"]);
+                assert(top_screw_config.contains("direction"));
+                top_screw.direction = scalarxyz_obj::read_value_from_json_array(top_screw_config["direction"]);
+            }
+            { // side screw
+                auto &side_screw_config = screw_config["side"];
+                assert(side_screw_config.contains("origin"));
+                side_screw.origin = scalarxyz_obj::read_value_from_json_array(side_screw_config["origin"]);
+                assert(side_screw_config.contains("direction"));
+                side_screw.direction = scalarxyz_obj::read_value_from_json_array(side_screw_config["direction"]);
+            }
+        }
+
+        void load_start_config(const nlohmann::json &config) {
+            assert(config.contains("input_config"));
+            { // output config
+                auto &output_config = config["output_config"];
+                current_control_mode_index = LOAD_VARIABLE_INDEX2(output_config, u64int_obj, "current_control_mode");
+                is_moving_index = TRY_LOAD_VARIABLE_INDEX2(output_config, bool_obj, "is_moving");
+            }
+            { // input config
+                // control mode
+                auto &input_config = config["input_config"];
+                auto control_mode_str = LOAD_STRING_ITEM2(input_config, "control_mode");
+                current_control_mode = control_mode_translator->translate(control_mode_str);
+                UPDATE_VARIABLE_VAL(u64int_obj, current_control_mode_index, (uint8_t) current_control_mode);
+                // controller parameters
+                max_velocity = TRY_LOAD_FLOAT_ITEM2(input_config, "max_velocity", default_max_velocity);
+                controller_gain = TRY_LOAD_FLOAT_ITEM2(input_config, "controller_gain", default_controller_gain);
+                // control input
+                switch (current_control_mode) {
+                    case control_mode_type::DIRECT: {
+                        target_motor_speed_index
+                                = LOAD_VARIABLE_INDEX2(input_config, array6_obj, "target_motor_velocity");
+                        break;
+                    }
+                    case control_mode_type::CARTESIAN: {
+                        current_tcp_pose_index = LOAD_VARIABLE_INDEX2(input_config, transform_obj, "current_tcp");
+                        target_tcp_pose_index = LOAD_VARIABLE_INDEX2(input_config, transform_obj, "target_tcp");
+                        break;
+                    }
+                    default: {
+                        assert(false);
+                        return;
+                    }
+                }
+            }
+        }
+    };
+
+    awaitable<bool> fracture_robot_interface::on_init(const nlohmann::json &config) noexcept {
+        pimpl->load_init_config(config);
+        try {
+            pimpl->setup_connection(config);
+        } catch (std::exception &e) {
+            SPDLOG_ERROR("Failed to init fracture robot interface: {}", e.what());
+            co_return false;
+        }
+        co_return true;
+    }
+
+    awaitable<bool> fracture_robot_interface::on_start(const nlohmann::json &config) noexcept {
+        pimpl->load_start_config(config);
+        pimpl->create_control_worker();
+        co_return true;
+    }
+
+    awaitable<void> fracture_robot_interface::on_stop() noexcept {
+        SAFE_RESET_WORKER(pimpl->control_worker)
+        // update status
+        try_update_variable_value<bool_obj>(pimpl->is_moving_index, false);
+        co_return;
+    }
+
+    awaitable<void> fracture_robot_interface::on_reset() noexcept {
+        pimpl->conn.reset(nullptr);
+        co_return;
+    }
+
+    fracture_robot_interface::fracture_robot_interface()
+            : pimpl(std::make_unique<impl>()) {
+        pimpl->q_this = this;
+    }
+
+    fracture_robot_interface::~fracture_robot_interface() = default;
+
+}

src/sensor/optoforce/optoforce_daq.cpp

@@ -154,9 +154,7 @@ namespace sophiar {
             conn.reset(new serial_port(*global_context));
 
             // open serial port
-            assert(config.contains("com_port"));
-            assert(config["com_port"].is_string());
-            auto com_port_name = config["com_port"].get<std::string>();
+            auto com_port_name = LOAD_STRING_ITEM("com_port");
             conn->open(com_port_name);
             assert(conn->is_open());
 

src/utility/config_utility.hpp

@@ -62,6 +62,15 @@
         _val; \
     })
 
+#define TRY_LOAD_FLOAT_ITEM2(config, 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/singleton_helper.hpp

@@ -0,0 +1,28 @@
+#ifndef SOPHIAR2_SINGLETON_HELPER_HPP
+#define SOPHIAR2_SINGLETON_HELPER_HPP
+
+#include <type_traits>
+
+namespace sophiar {
+
+    template<typename DeriveT>
+    class singleton_helper {
+    public:
+        using this_type = singleton_helper<DeriveT>;
+
+        virtual void initialize() = 0;
+
+        static auto get_instance() {
+            static DeriveT *instance = nullptr;
+            if (instance == nullptr) {
+                instance = new DeriveT{};
+                instance->initialize();
+            }
+            return instance;
+        }
+
+    };
+
+}
+
+#endif //SOPHIAR2_SINGLETON_HELPER_HPP

src/utility/variable_helper.hpp

@@ -39,10 +39,13 @@ namespace sophiar {
             UPDATE_VARIABLE_VAL_WITH_TS(SmallObjType, var_index, val, ts);
         }
 
-        void manual_sync() {
+        // return true if received new value
+        bool manual_sync() {
             if (watcher.try_wait()) { // new value available
                 val_ptr = QUERY_VARIABLE(SmallObjType, var_index);
+                return true;
             }
+            return false;
         }
 
         pointer_type get_value() {

tests/CMakeLists.txt

@@ -21,7 +21,7 @@ target_link_libraries(test_utility ${Boost_LIBRARIES} ${EXTRA_LIBS})
 file(GLOB_RECURSE TEST_ALGORITHM_SRC_FILES ./algorithm/*.cpp)
 add_executable(test_algorithm
         ${TEST_ALGORITHM_SRC_FILES}
-        ../src/algorithm/transform_tree.cpp
+        ${ALGORITHM_IMPL_FILES}
         #        ${ALGORITHM_IMPL_FILES}
         ${CORE_IMPL_FILES})
 target_compile_definitions(test_algorithm PUBLIC SOPHIAR_TEST SOPHIAR_TEST_ALGORITHM)

tests/data/fracture_robot_interface_config.json

@@ -0,0 +1,58 @@
+{
+  "controller_port": 5277,
+  "variable_list": [
+    {
+      "name": "ur_joint_target_q",
+      "type": "array6_obj"
+    }
+  ],
+  "object_list": [
+    {
+      "type": "fracture_robot_interface",
+      "name": "frac_robot",
+      "init_config": {
+        "com_port": "/dev/ttyACM1",
+        "screw": {
+          "top": {
+            "origin": [
+              0,
+              0,
+              0
+            ],
+            "direction": [
+              0,
+              1,
+              0
+            ]
+          },
+          "side": {
+            "origin": [
+              0,
+              0,
+              0
+            ],
+            "direction": [
+              0,
+              0,
+              -1
+            ]
+          }
+        }
+      },
+      "start_config": {
+        "input_config": {
+          "max_velocity": 5.0,
+          "controller_gain": 3.0,
+          "control_mode": "cartesian",
+          "current_tcp": "tcp_in_robot_base",
+          "target_tcp": "target_tcp_in_robot_base",
+          "target_motor_velocity": "motor_velocity"
+        },
+        "output_config": {
+          "current_control_mode": "robot_control_mode",
+          "is_moving": "robot_is_moving"
+        }
+      }
+    }
+  ]
+}