Make UR unit compatible with sophiar.

src/algorithm/algorithm_defs.cpp

@@ -3,19 +3,13 @@
 #include "algorithm/transform_utility.hpp"
 #include "core/global_defs.h"
 
-#if !SOPHIAR_TEST || SOPHIAR_TEST_ALGORITHM
-
 DEFAULT_TRISTATE_OBJ_DEF(transform_tree)
 DEFAULT_TRISTATE_OBJ_DEF(transform_stabilizer)
 DEFAULT_TRISTATE_OBJ_DEF(landmark_registration)
 
-#endif
-
 namespace sophiar {
 
     void register_algorithms() {
-#if !SOPHIAR_TEST || SOPHIAR_TEST_ALGORITHM
-
         REGISTER_TYPE(transform_tree);
         REGISTER_TYPE(transform_stabilizer);
         REGISTER_TYPE(landmark_registration);
@@ -29,8 +23,6 @@ namespace sophiar {
         using scalarxyz_transformer = simple_tristate_obj_wrapper<make_scalarxyz_transformer>;
         REGISTER_TYPE(transform_inverter);
         REGISTER_TYPE(scalarxyz_transformer);
-
-#endif
     }
 
 }

src/core/global_defs.cpp

@@ -81,7 +81,13 @@ namespace sophiar {
     void register_object_types() {
 
         REGISTER_TYPE(empty_object);
+
+#if !SOPHIAR_TEST || SOPHIAR_TEST_ALGORITHM
+
         register_algorithms();
+
+#endif
+
         register_controllers();
 
 #if !SOPHIAR_TEST || SOPHIAR_TEST_ROBOT

src/robot/ur/scripts/ur_script.hpp

@@ -0,0 +1,57 @@
+#ifndef SOPHIAR2_UR_SCRIPT_HPP
+#define SOPHIAR2_UR_SCRIPT_HPP
+
+#include <string_view>
+
+namespace sophiar {
+
+    static std::string_view ur_script_data =
+            "ZGVmIG15UHJvZygpOgogICAgZGVmIG5vTW90aW9uKCk6CiAgICAgICAgd3JpdGVf"
+            "b3V0cHV0X2Jvb2xlYW5fcmVnaXN0ZXIoNjQsIEZhbHNlKQogICAgICAgIHN0b3Bq"
+            "KDEuNCkKICAgICAgICBzeW5jKCkKICAgICAgICByZXR1cm4gTm9uZQogICAgZW5k"
+            "CgogICAgYSA9IFswLCAwLCAwLCAwLCAwLCAwXQogICAgYiA9IFswLCAwLCAwLCAw"
+            "LCAwLCAwXQoKICAgIHJlc1RpbWUgPSAwLjEKICAgIGxhc3RDbWRJZCA9IDAKCgl3"
+            "aGlsZSBUcnVlOgoJICAgIGVudGVyX2NyaXRpY2FsCgkJbW92ZVR5cGUgPSByZWFk"
+            "X2lucHV0X2ludGVnZXJfcmVnaXN0ZXIoMjQpCgkJY21kSWQgPSByZWFkX2lucHV0"
+            "X2ludGVnZXJfcmVnaXN0ZXIoMjUpCgoJCWkgPSAwCgkJd2hpbGUgaSA8IDY6CgkJ"
+            "CWFbaV0gPSByZWFkX2lucHV0X2Zsb2F0X3JlZ2lzdGVyKGkgKyAyNCkKCQkJaSA9"
+            "IGkgKyAxCgkJZW5kCgogICAgICAgIGkgPSAwCiAgICAgICAgd2hpbGUgaSA8IDY6"
+            "CiAgICAgICAgICAgIGJbaV0gPSByZWFkX2lucHV0X2Zsb2F0X3JlZ2lzdGVyKGkg"
+            "KyAzMCkKICAgICAgICAgICAgaSA9IGkgKyAxCiAgICAgICAgZW5kCgogICAgICAg"
+            "IHBhcmFtQSA9IHJlYWRfaW5wdXRfZmxvYXRfcmVnaXN0ZXIoMzYpCiAgICAgICAg"
+            "cGFyYW1WID0gcmVhZF9pbnB1dF9mbG9hdF9yZWdpc3RlcigzNykKICAgICAgICBl"
+            "eGl0X2NyaXRpY2FsCgogICAgICAgIHRjcFBvc2UgPSBwW2FbMF0sIGFbMV0sIGFb"
+            "Ml0sIGFbM10sIGFbNF0sIGFbNV1dCgkJcGFyYW1Qb3NlID0gcFtiWzBdLCBiWzFd"
+            "LCBiWzJdLCBiWzNdLCBiWzRdLCBiWzVdXQoJCXBhcmFtQXJyYXkgPSBbYlswXSwg"
+            "YlsxXSwgYlsyXSwgYlszXSwgYls0XSwgYls1XV0KCiAgICAgICAgaWYgY21kSWQg"
+            "IT0gbGFzdENtZElkOgogICAgICAgICAgICBzZXRfdGNwKHRjcFBvc2UpCiAgICAg"
+            "ICAgICAgIHJlc1RpbWUgPSAwLjEKICAgICAgICBlbmQKCiAgICAgICAgaWYgbW92"
+            "ZVR5cGUgPT0gMDoKICAgICAgICAgICAgbm9Nb3Rpb24oKQogICAgICAgIGVsaWYg"
+            "bW92ZVR5cGUgPT0gMToKICAgICAgICAgICAgaWYgY21kSWQgPT0gbGFzdENtZElk"
+            "OgogICAgICAgICAgICAgICAgbm9Nb3Rpb24oKQogICAgICAgICAgICBlbHNlOgog"
+            "ICAgICAgICAgICAgICAgd3JpdGVfb3V0cHV0X2Jvb2xlYW5fcmVnaXN0ZXIoNjQs"
+            "IFRydWUpCiAgICAgICAgICAgICAgICBtb3ZlaihwYXJhbUFycmF5LCBwYXJhbUEs"
+            "IHBhcmFtVikKICAgICAgICAgICAgZW5kCiAgICAgICAgZWxpZiBtb3ZlVHlwZSA9"
+            "PSAyOgogICAgICAgICAgICBpZiBjbWRJZCA9PSBsYXN0Q21kSWQ6CiAgICAgICAg"
+            "ICAgICAgICBub01vdGlvbigpCiAgICAgICAgICAgIGVsc2U6CiAgICAgICAgICAg"
+            "ICAgICB3cml0ZV9vdXRwdXRfYm9vbGVhbl9yZWdpc3Rlcig2NCwgVHJ1ZSkKICAg"
+            "ICAgICAgICAgICAgIG1vdmVsKHBhcmFtUG9zZSwgcGFyYW1BLCBwYXJhbVYpCiAg"
+            "ICAgICAgICAgIGVuZAogICAgICAgIGVsaWYgbW92ZVR5cGUgPT0gMzoKICAgICAg"
+            "ICAgICAgaWYgcmVzVGltZSA8IDA6CiAgICAgICAgICAgICAgICBub01vdGlvbigp"
+            "CiAgICAgICAgICAgIGVsc2U6CiAgICAgICAgICAgICAgICB3cml0ZV9vdXRwdXRf"
+            "Ym9vbGVhbl9yZWdpc3Rlcig2NCwgVHJ1ZSkKICAgICAgICAgICAgICAgIHNwZWVk"
+            "aihwYXJhbUFycmF5LCBwYXJhbUEsIGdldF9zdGVwdGltZSgpKQogICAgICAgICAg"
+            "ICAgICAgcmVzVGltZSA9IHJlc1RpbWUgLSBnZXRfc3RlcHRpbWUoKQogICAgICAg"
+            "ICAgICBlbmQKICAgICAgICBlbGlmIG1vdmVUeXBlID09IDQ6CiAgICAgICAgICAg"
+            "IGlmIHJlc1RpbWUgPCAwOgogICAgICAgICAgICAgICAgbm9Nb3Rpb24oKQogICAg"
+            "ICAgICAgICBlbHNlOgogICAgICAgICAgICAgICAgd3JpdGVfb3V0cHV0X2Jvb2xl"
+            "YW5fcmVnaXN0ZXIoNjQsIFRydWUpCiAgICAgICAgICAgICAgICBzcGVlZGwocGFy"
+            "YW1BcnJheSwgcGFyYW1BLCBnZXRfc3RlcHRpbWUoKSkKICAgICAgICAgICAgICAg"
+            "IHJlc1RpbWUgPSByZXNUaW1lIC0gZ2V0X3N0ZXB0aW1lKCkKICAgICAgICAgICAg"
+            "ZW5kCiAgICAgICAgZW5kCgogICAgICAgIGxhc3RDbWRJZCA9IGNtZElkCiAgICAg"
+            "ICAgd3JpdGVfb3V0cHV0X2ludGVnZXJfcmVnaXN0ZXIoMjQsIGxhc3RDbWRJZCkK"
+            "ICAgIGVuZAplbmQKCm15UHJvZygp";
+
+}
+
+#endif //SOPHIAR2_UR_SCRIPT_HPP

src/robot/ur/ur_interface.cpp

@@ -1,10 +1,14 @@
 #include "core/basic_obj_types.hpp"
 #include "robot/ur/ur_defs.h"
+#include "robot/ur/scripts/ur_script.hpp"
+#include "third_party/base64.h"
+#include "third_party/scope_guard.hpp"
 #include "utility/bit_operations.hpp"
 #include "utility/config_utility.hpp"
 #include "utility/coro_worker.hpp"
 #include "utility/coro_worker_helper_func.hpp"
 #include "utility/debug_utility.hpp"
+#include "utility/dynamic_pool.hpp"
 #include "utility/name_translator.hpp"
 #include "utility/variable_helper.hpp"
 #include "utility/versatile_buffer2.hpp"
@@ -15,7 +19,6 @@
 #include <boost/asio/detached.hpp>
 #include <boost/asio/use_awaitable.hpp>
 #include <boost/asio/write.hpp>
-#include <boost/iostreams/device/mapped_file.hpp>
 #include <boost/smart_ptr/scoped_ptr.hpp>
 #include <boost/system/error_code.hpp>
 
@@ -40,7 +43,6 @@ namespace sophiar {
     using boost::asio::co_spawn;
     using boost::asio::detached;
     using boost::asio::use_awaitable;
-    using boost::iostreams::mapped_file;
     using boost::scoped_ptr;
     using boost::system::error_code;
 
@@ -331,6 +333,42 @@ namespace sophiar {
             }
         }
 
+        static void unit_ur_to_sophiar(Eigen::Isometry3d &trans) {
+            trans.translation() *= 1000; // m -> mm
+        }
+
+        static void unit_ur_to_sophiar(vector6d_type &arr) {
+            for (int i = 0; i < 3; ++i) { // only for the linear velocity part
+                arr[i] *= 1000; // m -> mm
+            }
+        }
+
+        static void unit_ur_to_sophiar(Eigen::Vector3d &vec) {
+            vec *= 1000; // m -> mm
+        }
+
+        static void unit_ur_to_sophiar(double &val) {
+            val *= 1000; // m -> mm
+        }
+
+        static void unit_sophiar_to_ur(Eigen::Isometry3d &trans) {
+            trans.translation() *= 0.001; // mm -> m
+        }
+
+        static void unit_sophiar_to_ur(vector6d_type &arr) {
+            for (int i = 0; i < 3; ++i) { // only for the linear velocity part
+                arr[i] *= 0.001; // mm -> m
+            }
+        }
+
+        static void unit_sophiar_to_ur(Eigen::Vector3d &vec) {
+            vec *= 0.001; // mm -> m
+        }
+
+        static void unit_sophiar_to_ur(double &val) {
+            val *= 0.001; // mm -> m
+        }
+
         static Eigen::Isometry3d identity_transform;
         static vector6d_type identity_transform_vec;
 
@@ -340,14 +378,13 @@ namespace sophiar {
         static constexpr uint16_t rtde_port = 30004;
         static constexpr uint16_t urscript_port = 30002;
         static constexpr uint16_t rtde_protocol_version = 2;
-        static constexpr auto urscript_path = "scripts/URScript.script";
 
         static constexpr auto ur_endian = boost::endian::order::big;
 
         static constexpr double joint_acceleration_default = 1.4; // rad/s^2
-        static constexpr double tcp_acceleration_default = 1.2; // m/s^2
+        static constexpr double tcp_acceleration_default = 1200; // mm/s^2
         static constexpr double joint_speed_default = 1.05; // rad/s
-        static constexpr double tcp_speed_default = 0.25; // m/s
+        static constexpr double tcp_speed_default = 250; // mm/s
 
         ur_interface *q_this = nullptr;
 
@@ -497,10 +534,12 @@ namespace sophiar {
             tcp::socket urscript_socket(*global_context);
             co_await urscript_socket.async_connect({ur_ip, urscript_port}, use_awaitable);
             // send script file
-            auto script_file = mapped_file(urscript_path, boost::iostreams::mapped_file::readonly);
-            assert(script_file.is_open());
+            auto script_len = macaron::Base64::Decode(ur_script_data, nullptr);
+            auto script_data = (char *) global_dynamic_pool->allocate(script_len);
+            auto closer = sg::make_scope_guard([&]() { global_dynamic_pool->deallocate(script_data, script_len); });
+            macaron::Base64::Decode(ur_script_data, script_data);
             co_await async_write(urscript_socket,
-                                 boost::asio::buffer(script_file.const_data(), script_file.size()),
+                                 boost::asio::buffer(script_data, script_len),
                                  use_awaitable);
             co_return;
         }
@@ -520,7 +559,7 @@ namespace sophiar {
 
         void handle_data_package(income_reader_type &reader) {
             auto content = ur_status_content{};
-            const auto &status = content.content;
+            auto &status = content.content;
             content.fill_from(reader);
             assert(content.recipe_id == outputs_recipe_id);
 
@@ -531,12 +570,21 @@ namespace sophiar {
             try_update_variable_value<array6_obj>(target_qdd_index, status.target_qdd, ts);
             try_update_variable_value<array6_obj>(actual_q_index, status.actual_q, ts);
             try_update_variable_value<array6_obj>(actual_qd_index, status.actual_qd, ts);
+
             vector6d_to_transform(status.target_TCP_pose, trans_tmp);
+            unit_ur_to_sophiar(trans_tmp);
             try_update_variable_value<transform_obj>(target_pose_index, trans_tmp, ts);
+
+            unit_ur_to_sophiar(status.target_TCP_speed);
             try_update_variable_value<array6_obj>(target_speed_index, status.target_TCP_speed, ts);
+
             vector6d_to_transform(status.actual_TCP_pose, trans_tmp);
+            unit_ur_to_sophiar(trans_tmp);
             try_update_variable_value<transform_obj>(actual_pose_index, trans_tmp, ts);
+
+            unit_ur_to_sophiar(status.actual_TCP_speed);
             try_update_variable_value<array6_obj>(actual_speed_index, status.actual_TCP_speed, ts);
+
             try_update_variable_value<bool_obj>(is_controllable_index, determine_is_controllable(status), ts);
             try_update_variable_value<bool_obj>(is_moving_index, status.is_moving, ts);
         }
@@ -678,6 +726,11 @@ namespace sophiar {
                         try_query_variable_value<double_obj>(
                                 tcp_speed_limit_index, tcp_speed_default);
 
+                unit_sophiar_to_ur(cmd_content->tcp_pose);
+                unit_sophiar_to_ur(cmd_content->target);
+                unit_sophiar_to_ur(cmd_content->param_a);
+                unit_sophiar_to_ur(cmd_content->param_v);
+
                 co_await send_rtde_packet(*content);
                 co_return true;
             };
@@ -712,6 +765,10 @@ namespace sophiar {
                         try_query_variable_value<double_obj>(
                                 tcp_acceleration_limit_index, tcp_acceleration_default);
 
+                unit_sophiar_to_ur(cmd_content->tcp_pose);
+                unit_sophiar_to_ur(cmd_content->target);
+                unit_sophiar_to_ur(cmd_content->param_a);
+
                 co_await send_rtde_packet(*content);
                 co_return true;
             };

tests/data/ur_interface_config.json

@@ -71,7 +71,8 @@
     },
     {
       "name": "robot_tcp_speed_limit",
-      "type": "double_obj"
+      "type": "double_obj",
+      "value": 10
     },
     {
       "name": "robot_tcp_acceleration_limit",
@@ -91,7 +92,7 @@
       "type": "ur_interface",
       "name": "ur",
       "init_config": {
-        "address": "192.168.38.141"
+        "address": "10.0.0.15"
       },
       "start_config": {
         "output_config": {
@@ -114,7 +115,7 @@
           "current_control_mode": "robot_control_mode"
         },
         "input_config": {
-          "control_mode": "tcp_pose",
+          "control_mode": "tcp_speed",
           "tcp_offset_pose": "robot_tcp_offset_pose",
           "target_tcp_pose": "robot_target_tcp_pose",
           "target_tcp_speed": "robot_target_tcp_speed",

tests/robot/ur_interface.cpp

@@ -82,7 +82,7 @@ awaitable<void> test_tcp_pose_control() {
     BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
     auto next_tcp_pose = transform_obj::new_instance();
     next_tcp_pose->value = tcp_pose->value;
-    next_tcp_pose->value.translation().z() += 0.1;
+    next_tcp_pose->value.translation().z() -= 50;
     UPDATE_VARIABLE(transform_obj, target_index, next_tcp_pose);
     co_await coro_wait_for_variable_value<bool_obj>(var_index, true);
     BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == true);
@@ -90,7 +90,7 @@ awaitable<void> test_tcp_pose_control() {
     BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
     next_tcp_pose = transform_obj::new_instance();
     next_tcp_pose->value = tcp_pose->value;
-    next_tcp_pose->value.translation().z() -= 0.1;
+    next_tcp_pose->value.translation().z() += 50;
     UPDATE_VARIABLE(transform_obj, target_index, next_tcp_pose);
     co_await coro_wait_for_variable_value<bool_obj>(var_index, true);
     BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == true);
@@ -111,7 +111,7 @@ awaitable<void> test_tcp_speed_control() {
     for (int i = 0; i < 40; ++i) {
         auto next_tcp_speed = array6_obj::new_instance();
         next_tcp_speed->value.fill({});
-        next_tcp_speed->value[3] = 0.2;
+        next_tcp_speed->value[3] = -0.05;
         UPDATE_VARIABLE(array6_obj, target_index, next_tcp_speed);
         co_await coro_sleep(50ms);
     }
@@ -120,7 +120,7 @@ awaitable<void> test_tcp_speed_control() {
     for (int i = 0; i < 40; ++i) {
         auto next_tcp_speed = array6_obj::new_instance();
         next_tcp_speed->value.fill({});
-        next_tcp_speed->value[3] = -0.2;
+        next_tcp_speed->value[3] = 0.05;
         UPDATE_VARIABLE(array6_obj, target_index, next_tcp_speed);
         co_await coro_sleep(50ms);
     }
@@ -135,6 +135,6 @@ BOOST_AUTO_TEST_CASE(test_ur_interface) {
     BOOST_TEST(config_file.is_open());
     auto config = nlohmann::json::parse(config_file);
     BOOST_TEST(initialize(config));
-    co_spawn(*global_context, test_tcp_pose_control(), detached);
+    co_spawn(*global_context, test_tcp_speed_control(), detached);
     global_context->run();
 }