实现了 versatile_data

src/core/datanode_base.h

@@ -4,6 +4,7 @@
 #include "core/small_obj.hpp"
 #include "core/timestamp_helper.hpp"
 #include "core/tristate_obj.h"
+#include "core/types/versatile_data.hpp"
 
 #include <chrono>
 #include <cstring>
@@ -11,42 +12,15 @@
 
 namespace sophiar {
 
-    // TODO 修改内容
-    struct data_packet_content {
-        static constexpr size_t DATA_PACKET_LENGTH = 14;
-
-        uint32_t user_data;
-        timestamp_type timestamp;
-        double data[DATA_PACKET_LENGTH];
-
-        auto &operator[](size_t index) {
-            assert(index < DATA_PACKET_LENGTH);
-            return data[index];
-        }
-
-        const auto &operator[](size_t index) const {
-            assert(index < DATA_PACKET_LENGTH);
-            return data[index];
-        }
-
-        void refresh() {
-            user_data = 0;
-            timestamp = current_timestamp();
-            std::memset(data, 0x00, sizeof(double) * DATA_PACKET_LENGTH);
-        }
-    };
-
     template<typename FreqTag>
     class datanode_base : public tristate_obj<FreqTag> {
     public:
 
-        struct data_packet : public data_packet_content,
+        struct data_packet : public versatile_data,
                              public small_obj<data_packet> {
 
-            void copy_content(const data_packet_content &content) {
-                std::memcpy(dynamic_cast<data_packet_content *>(this),
-                            &content,
-                            sizeof(data_packet_content));
+            void copy_content(const versatile_data &other) {
+                std::memcpy(dynamic_cast<versatile_data *>(this), &other, sizeof(versatile_data));
             }
 
         };

src/core/transform_tree.cpp

@@ -0,0 +1,20 @@
+#include "transform_tree.h"
+
+namespace sophiar {
+
+    struct transform_tree::impl {
+
+        struct node_type {
+
+        };
+
+        node_type nodes[MAX_NODE_CNT];
+
+    };
+
+    transform_tree::transform_tree()
+            : pimpl(std::make_unique<impl>()) {}
+
+    transform_tree::~transform_tree() = default;
+
+}

src/core/transform_tree.h

@@ -0,0 +1,30 @@
+#ifndef SOPHIAR2_TRANSFORM_TREE_H
+#define SOPHIAR2_TRANSFORM_TREE_H
+
+#include <Eigen/Geometry>
+
+#include <memory>
+
+namespace sophiar {
+
+    class transform_tree {
+    public:
+
+        using trans_type = Eigen::Isometry3d;
+
+        static constexpr auto MAX_NODE_CNT = 32;
+
+        transform_tree();
+
+        ~transform_tree();
+
+    private:
+
+        struct impl;
+        std::unique_ptr<impl> pimpl;
+
+    };
+
+}
+
+#endif //SOPHIAR2_TRANSFORM_TREE_H

src/core/types/geometry_types.hpp

@@ -0,0 +1,138 @@
+#ifndef SOPHIAR2_GEOMETRY_TYPES_HPP
+#define SOPHIAR2_GEOMETRY_TYPES_HPP
+
+#include "core/types/versatile_data.hpp"
+
+#include <Eigen/Geometry>
+
+#include <cassert>
+
+namespace sophiar {
+
+    struct coordinate_pose_compact;
+
+    // translation unit: mm
+    struct coordinate_pose : public Eigen::Isometry3d {
+        EIGEN_DERIVED(coordinate_pose, Eigen::Isometry3d);
+
+        static constexpr auto type_id = (uint32_t) versatile_type_id::COORDINATE_POSE;
+
+    protected:
+
+        void fill_from_versatile_data(const versatile_data &data) {
+            auto rot_matrix = data.view_as_matrix<3, 3, 0>();
+            auto trans_vec = data.view_as_matrix<3, 1, 9>();
+            this->linear().noalias() = rot_matrix;
+            this->translation().noalias() = trans_vec;
+        }
+
+        void write_to_versatile_data(versatile_data &data) const {
+            auto rot_matrix = data.view_as_matrix<3, 3, 0>();
+            auto trans_vec = data.view_as_matrix<3, 1, 9>();
+            rot_matrix.noalias() = this->linear();
+            trans_vec.noalias() = this->translation();
+        }
+
+    private:
+
+        friend class versatile_data;
+
+    };
+
+    // translation unit: mm
+    // quaternion form: [x, y, z, w]
+    struct coordinate_pose_compact : public Eigen::Isometry3d {
+        EIGEN_DERIVED(coordinate_pose_compact, Eigen::Isometry3d);
+
+        static constexpr auto type_id = (uint32_t) versatile_type_id::COORDINATE_POSE_COMPACT;
+
+    protected:
+
+        void fill_from_versatile_data(const versatile_data &data) {
+            auto trans_part = data.view_as_matrix<3, 1, 0>();
+            auto quat_part = data.view_as_matrix<4, 1, 3>();
+            this->view_as_base() = Eigen::Translation3d(trans_part) * Eigen::Quaterniond(quat_part);
+        }
+
+        void write_to_versatile_data(versatile_data &data) const {
+            auto trans_part = data.view_as_matrix<3, 1, 0>();
+            auto quat_part = data.view_as_matrix<4, 1, 3>();
+            trans_part.noalias() = this->translation();
+            quat_part.noalias() = Eigen::Quaterniond(this->rotation()).coeffs();
+        }
+
+    private:
+
+        friend class versatile_data;
+
+    };
+
+    // angular unit: rad
+    // translation unit: mm
+    struct coordinate_twist : public default_versatile_wrapper<
+            versatile_type_id::COORDINATE_TWIST, 6, 1> {
+
+        auto angular_part() {
+            return this->block<3, 1>(0, 0);
+        }
+
+        auto angular_part() const {
+            return this->block<3, 1>(0, 0);
+        }
+
+        auto linear_part() {
+            return this->block<3, 1>(3, 0);
+        }
+
+        auto linear_part() const {
+            return this->block<3, 1>(3, 0);
+        }
+
+        // 从点 rot_origin 按角速度 angular_velocity 旋转
+        static auto from_rot_axis(const Eigen::Vector3d &angular_velocity,
+                                  const Eigen::Vector3d &rot_origin = Eigen::Vector3d::Zero()) {
+            coordinate_twist twist;
+            twist.angular_part().noalias() = angular_velocity;
+            twist.linear_part().noalias() = -angular_velocity.cross(rot_origin);
+            return twist;
+        }
+
+    };
+
+    auto make_skew(const Eigen::Vector3d &vec) { // TODO 不知道怎么优化
+        return Eigen::Matrix3d{{0,       -vec(2), vec(1)},
+                               {vec(2),  0,       -vec(0)},
+                               {-vec(1), vec(0),  0}};
+    }
+
+    auto operator*(const Eigen::Isometry3d &trans, const coordinate_twist &old_twist) { // TODO 不知道怎么优化
+        coordinate_twist new_twist;
+        auto rot_matrix = trans.linear();
+        auto trans_vec = trans.translation();
+        auto old_angular_part = old_twist.angular_part();
+        auto old_linear_part = old_twist.linear_part();
+        new_twist.angular_part().noalias() = rot_matrix * old_angular_part;
+        new_twist.linear_part().noalias() = trans_vec.cross(rot_matrix * old_angular_part) +
+                                            rot_matrix * old_linear_part;
+        return new_twist;
+    }
+
+    // unit: mm
+    struct point : public default_versatile_wrapper<
+            versatile_type_id::POINT, 3, 1> {
+    };
+
+    // unit: mm/s
+    struct point_velocity : public default_versatile_wrapper<
+            versatile_type_id::POINT_VELOCITY, 3, 1> {
+    };
+
+    auto operator*(const Eigen::Isometry3d &trans, const point_velocity &old_vec) {
+        point_velocity new_vec;
+        new_vec.noalias() = trans.linear() * old_vec;
+        return new_vec;
+    }
+
+}
+
+#endif //SOPHIAR2_GEOMETRY_TYPES_HPP

src/core/types/physical_types.hpp

@@ -0,0 +1,42 @@
+#ifndef SOPHIAR2_PHYSICAL_TYPES_HPP
+#define SOPHIAR2_PHYSICAL_TYPES_HPP
+
+#include "core/types/versatile_data.hpp"
+
+#include <Eigen/Geometry>
+
+namespace sophiar {
+
+    // force unit: N
+    // torque unit: Nm
+    struct force_torque_xyz : public default_versatile_wrapper<
+            versatile_type_id::FORCE_TORQUE_XYZ, 6, 1> {
+
+        auto force_part() {
+            return this->block<3, 1>(0, 0);
+        }
+
+        auto force_part() const {
+            return this->block<3, 1>(0, 0);
+        }
+
+        auto torque_part() {
+            return this->block<3, 1>(3, 0);
+        }
+
+        auto torque_part() const {
+            return this->block<3, 1>(3, 0);
+        }
+
+    };
+
+    auto operator*(const Eigen::Isometry3d &trans, const force_torque_xyz &ft_old) {
+        force_torque_xyz ft_new;
+        ft_new.force_part().noalias() = trans.linear() * ft_old.force_part();
+        ft_new.torque_part().noalias() = trans.linear() * ft_old.torque_part();
+        return ft_new;
+    }
+
+}
+
+#endif //SOPHIAR2_PHYSICAL_TYPES_HPP

src/core/types/robot_types.hpp

@@ -0,0 +1,18 @@
+#ifndef SOPHIAR2_ROBOT_TYPES_HPP
+#define SOPHIAR2_ROBOT_TYPES_HPP
+
+#include "core/types/versatile_data.hpp"
+
+namespace sophiar {
+
+    struct angle_arr6 : default_versatile_wrapper<
+            versatile_type_id::ANGLE_ARR6, 6, 1> {
+    };
+
+    struct angle_arr7 : default_versatile_wrapper<
+            versatile_type_id::ANGLE_ARR7, 7, 1> {
+    };
+
+}
+
+#endif //SOPHIAR2_ROBOT_TYPES_HPP

src/core/types/vectorx.hpp

@@ -1,431 +0,0 @@
-#ifndef SOPHIAR2_VECTORX_HTTP
-#define SOPHIAR2_VECTORX_HTTP
-
-#include "core/small_obj.hpp"
-#include "utility/versatile_buffer.hpp"
-
-#include <fmt/format.h>
-
-#include <Eigen/Core>
-
-#include <algorithm>
-#include <array>
-#include <cassert>
-#include <initializer_list>
-#include <stdexcept>
-#include <type_traits>
-#include <utility>
-
-namespace sophiar {
-
-    template<typename FloatType, size_t Length>
-    class vectorx;
-
-    template<typename FreqTag, typename FloatType, size_t Length>
-    class vectorx_obj;
-
-    template<typename FloatPointerType, size_t Length>
-    class vectorx_view {
-    public:
-
-        using FloatType = std::remove_pointer_t<FloatPointerType>;
-        using RealFloatType = std::remove_const_t<FloatType>;
-
-        static_assert(std::is_pointer_v<FloatPointerType>);
-        static_assert(std::is_arithmetic_v<FloatType>);
-
-        using eigen_row_vector_type = Eigen::Map<Eigen::Matrix<RealFloatType, 1, Length>>;
-        using eigen_const_row_vector_type = Eigen::Map<const Eigen::Matrix<RealFloatType, 1, Length>>;
-        using eigen_column_vector_type = Eigen::Map<Eigen::Matrix<RealFloatType, Length, 1>>;
-        using eigen_const_column_vector_type = Eigen::Map<const Eigen::Matrix<RealFloatType, Length, 1>>;
-
-        explicit vectorx_view(FloatPointerType other_data)
-                : data(other_data) {}
-
-        vectorx_view(vectorx<RealFloatType, Length> &other)
-                : data(other.m.data()) {}
-
-        template<typename ThisFloatType = FloatType,
-                typename = std::enable_if_t<std::is_const_v<ThisFloatType>>>
-        vectorx_view(const vectorx<RealFloatType, Length> &other)
-                : data(other.m.data()) {}
-
-        vectorx_view(const vectorx_view<RealFloatType *, Length> &other)
-                : data(other.data) {}
-
-        template<typename ThisFloatType = FloatType,
-                typename = std::enable_if_t<std::is_const_v<ThisFloatType>>>
-        vectorx_view(const vectorx_view<const RealFloatType *, Length> &other)
-                : data(other.data) {}
-
-        auto to_vectorx() const {
-            return vectorx<RealFloatType, Length>(*this);
-        }
-
-        template<typename FreqTag>
-        auto to_vectorx_obj() const {
-            return vectorx_obj<FreqTag, RealFloatType, Length>::new_instance(*this);
-        };
-
-        template<size_t Pos,
-                typename ThisFloatType = FloatType,
-                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>>
-        FloatType &at() {
-            static_assert(Pos < Length);
-            return data[Pos];
-        }
-
-        template<size_t Pos>
-        const FloatType &at() const {
-            static_assert(Pos < Length);
-            return data[Pos];
-        }
-
-        template<typename ThisFloatType = FloatType,
-                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>>
-        FloatType &at(size_t pos) {
-            assert(pos < Length);
-            return data[pos];
-        }
-
-        const FloatType &at(size_t pos) const {
-            assert(pos < Length);
-            return data[pos];
-        }
-
-        template<typename ThisFloatType = FloatType,
-                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>>
-        FloatType &operator[](size_t pos) {
-            return at(pos);
-        }
-
-        const FloatType &operator[](size_t pos) const {
-            return at(pos);
-        }
-
-        template<typename OtherFloatType, size_t OtherLength,
-                typename = std::enable_if_t<OtherLength < Length>>
-        auto fill(const vectorx<OtherFloatType, OtherLength> &other) {
-            copy_from_vectorx<OtherLength>(other);
-            return vectorx_view<FloatPointerType, Length - OtherLength>(data + OtherLength);
-        };
-
-        template<typename OtherFloatPointerType, size_t OtherLength,
-                typename = std::enable_if_t<OtherLength < Length>>
-        auto fill(const vectorx_view<OtherFloatPointerType, OtherLength> &other) {
-            copy_from_vectorx_view<OtherLength>(other);
-            return vectorx_view<FloatPointerType, Length - OtherLength>(data + OtherLength);
-        };
-
-        template<typename OtherFloatType, int OtherRows, int OtherColumns,
-                size_t CopyLength = OtherRows * OtherColumns,
-                typename = std::enable_if_t<CopyLength < Length>>
-        auto fill(const Eigen::Matrix<OtherFloatType, OtherRows, OtherColumns> &other) {
-            copy_from_eigen_matrix<CopyLength>(other);
-            return vectorx_view<FloatPointerType, Length - CopyLength>(data + CopyLength);
-        }
-
-        template<typename OtherFloatType, size_t OtherLength,
-                typename = std::enable_if_t<OtherLength < Length>>
-        auto fill(const OtherFloatType(&list)[OtherLength]) {
-            copy_from_list<OtherLength>(list);
-            return vectorx_view<FloatPointerType, Length - OtherLength>(data + OtherLength);
-        };
-
-        template<typename OtherFloatType, size_t OtherLength,
-                typename = std::enable_if_t<OtherLength == Length>>
-        void fill(const vectorx<OtherFloatType, OtherLength> &other) {
-            copy_from_vectorx<OtherLength>(other);
-        };
-
-        template<typename OtherFloatPointerType, size_t OtherLength,
-                typename = std::enable_if_t<OtherLength == Length>>
-        void fill(const vectorx_view<OtherFloatPointerType, OtherLength> &other) {
-            copy_from_vectorx_view<OtherLength>(other);
-        };
-
-        template<typename OtherFloatType, int OtherRows, int OtherColumns,
-                size_t CopyLength = OtherRows * OtherColumns,
-                typename = std::enable_if_t<CopyLength == Length>>
-        void fill(const Eigen::Matrix<OtherFloatType, OtherRows, OtherColumns> &other) {
-            copy_from_eigen_matrix<CopyLength>(other);
-        }
-
-        template<typename OtherFloatType, size_t OtherLength,
-                typename = std::enable_if_t<OtherLength == Length>>
-        void fill(const OtherFloatType(&list)[OtherLength]) {
-            copy_from_list<OtherLength>(list);
-        };
-
-        static constexpr uint16_t length() {
-            return sizeof(FloatType) * Length;
-        }
-
-        void write_to_buffer(versatile_buffer &buffer) const {
-            for (size_t i = 0; i < Length; ++i) {
-                buffer << data[i];
-            }
-        }
-
-        template<typename ThisFloatType = FloatType,
-                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>>
-        void fill_from_buffer(versatile_buffer &buffer) {
-            for (size_t i = 0; i < Length; ++i) {
-                buffer >> data[i];
-            }
-        }
-
-        template<typename ThisFloatType = FloatType,
-                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>>
-        auto as_eigen_row_view() {
-            return eigen_row_vector_type(data);
-        }
-
-        auto as_eigen_const_row_view() const {
-            return eigen_const_row_vector_type(data);
-        }
-
-        template<typename ThisFloatType = FloatType,
-                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>>
-        auto as_eigen_column_view() {
-            return eigen_column_vector_type(data);
-        }
-
-        auto as_eigen_const_column_view() const {
-            return eigen_const_column_vector_type(data);
-        }
-
-    private:
-        FloatPointerType data;
-
-        template<typename, size_t> friend
-        class vectorx_view;
-
-        template<typename, size_t> friend
-        class vectorx;
-
-        template<size_t CopyLength, typename OtherFloatType, size_t OtherLength,
-                typename ThisFloatType = FloatType,
-                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>>
-        void copy_from_vectorx(const vectorx<OtherFloatType, OtherLength> &other) {
-            static_assert(std::is_convertible_v<OtherFloatType, RealFloatType>);
-            static_assert(CopyLength <= Length);
-            std::copy_n(other.m.begin(), CopyLength, data);
-        }
-
-        template<size_t CopyLength, typename OtherFloatPointerType, size_t OtherLength,
-                typename ThisFloatType = FloatType,
-                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>>
-        void copy_from_vectorx_view(const vectorx_view<OtherFloatPointerType, OtherLength> &other) {
-            static_assert(std::is_convertible_v<
-                    std::remove_pointer_t<OtherFloatPointerType>, RealFloatType>);
-            static_assert(CopyLength <= Length);
-            std::copy_n(other.data, CopyLength, data);
-        }
-
-        template<size_t CopyLength, typename OtherFloatType, size_t OtherLength,
-                typename ThisFloatType = FloatType,
-                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>>
-        void copy_from_list(const OtherFloatType(&list)[OtherLength]) {
-            static_assert(std::is_convertible_v<OtherFloatType, RealFloatType>);
-            static_assert(CopyLength <= Length);
-            std::copy_n(list, CopyLength, data);
-        }
-
-        template<size_t CopyLength, typename OtherFloatType, int OtherRows, int OtherColumns,
-                typename ThisFloatType = FloatType,
-                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>>
-        void copy_from_eigen_matrix(const Eigen::Matrix<OtherFloatType, OtherRows, OtherColumns> &other) {
-            static_assert(std::is_convertible_v<OtherFloatType, RealFloatType>);
-            static_assert(OtherRows == 1 || OtherColumns == 1);
-            static_assert(OtherRows * OtherColumns == Length);
-            static_assert(CopyLength <= Length);
-            for (size_t i = 0; i < CopyLength; ++i) {
-                data[i] = other(i);
-            }
-        }
-    };
-
-    template<typename FloatType, size_t Length>
-    class vectorx {
-    public:
-
-        static_assert(std::is_arithmetic_v<FloatType>);
-
-        vectorx() = default;
-
-        vectorx(std::initializer_list<FloatType> list) {
-            copy_from_initial_list(list);
-        }
-
-        template<typename OtherFloatType>
-        explicit vectorx(const vectorx<OtherFloatType, Length> &other) {
-            copy_from_vectorx(other);
-        }
-
-        template<typename OtherFloatPointerType>
-        explicit vectorx(const vectorx_view<OtherFloatPointerType, Length> &other) {
-            copy_from_vectorx_view(other);
-        }
-
-        template<typename OtherFloatType, size_t OtherRows, size_t OtherColumns>
-        explicit vectorx(const Eigen::Matrix<OtherFloatType, OtherRows, OtherColumns> &other) {
-            copy_from_eigen_matrix(other);
-        }
-
-        vectorx &operator=(std::initializer_list<FloatType> list) {
-            copy_from_initial_list(list);
-            return *this;
-        }
-
-        template<typename OtherFloatType>
-        vectorx &operator=(const vectorx<OtherFloatType, Length> &other) {
-            copy_from_vectorx(other);
-            return *this;
-        }
-
-        template<typename OtherFloatPointerType>
-        vectorx &operator=(const vectorx_view<OtherFloatPointerType, Length> &other) {
-            copy_from_vector_view(other);
-            return *this;
-        }
-
-        template<typename OtherFloatType, int OtherRows, int OtherColumns>
-        vectorx &operator=(const Eigen::Matrix<OtherFloatType, OtherRows, OtherColumns> &other) {
-            copy_from_eigen_matrix(other);
-            return *this;
-        }
-
-        template<size_t Pos>
-        FloatType &at() {
-            static_assert(Pos < Length);
-            return m[Pos];
-        }
-
-        template<size_t Pos>
-        const FloatType &at() const {
-            static_assert(Pos < Length);
-            return m[Pos];
-        }
-
-        FloatType &at(size_t pos) {
-            assert(pos < Length);
-            return m.at(pos);
-        }
-
-        const FloatType &at(size_t pos) const {
-            assert(pos < Length);
-            return m.at(pos);
-        }
-
-        FloatType &operator[](size_t pos) {
-            assert(pos < Length);
-            return m[pos];
-        }
-
-        const FloatType &operator[](size_t pos) const {
-            assert(pos < Length);
-            return m[pos];
-        }
-
-        template<size_t StartPos = 0, size_t EndPos = Length>
-        auto get_view() {
-            static_assert(StartPos < EndPos && EndPos <= Length);
-            return vectorx_view<FloatType *, EndPos - StartPos>(m.data() + StartPos);
-        };
-
-        template<size_t StartPos = 0, size_t EndPos = Length>
-        auto get_view() const {
-            static_assert(StartPos < EndPos && EndPos <= Length);
-            return vectorx_view<const FloatType *, EndPos - StartPos>(m.data() + StartPos);
-        };
-
-        static constexpr uint16_t length() {
-            return sizeof(FloatType) * Length;
-        }
-
-        void write_to_buffer(versatile_buffer &buffer) const {
-            for (const auto &item: m) {
-                buffer << item;
-            }
-        }
-
-        void fill_from_buffer(versatile_buffer &buffer) {
-            for (auto &item: m) {
-                buffer >> item;
-            }
-        }
-
-    private:
-        std::array<FloatType, Length> m;
-
-        template<typename, size_t> friend
-        class vectorx_view;
-
-        void copy_from_initial_list(const std::initializer_list<FloatType> &list) {
-            assert(list.size() == Length);
-            std::copy_n(list.begin(), Length, m.begin());
-        }
-
-        template<typename OtherFloatType, int OtherRows, int OtherColumns>
-        void copy_from_eigen_matrix(const Eigen::Matrix<OtherFloatType, OtherRows, OtherColumns> &other) {
-            static_assert(std::is_convertible_v<OtherFloatType, FloatType>);
-            static_assert(OtherRows == 1 || OtherColumns == 1);
-            static_assert(OtherRows * OtherColumns == Length);
-            for (size_t i = 0; i < Length; ++i) {
-                m[i] = other(i);
-            }
-        }
-
-        template<typename OtherFloatType>
-        void copy_from_vectorx(const vectorx<OtherFloatType, Length> &other) {
-            static_assert(std::is_convertible_v<OtherFloatType, FloatType>);
-            m = other.m;
-        }
-
-        template<typename OtherFloatPointerType>
-        void copy_from_vectorx_view(const vectorx_view<OtherFloatPointerType, Length> &other) {
-            static_assert(std::is_convertible_v<
-                    std::remove_pointer_t<OtherFloatPointerType>, FloatType>);
-            std::copy_n(other.data, Length, m.begin());
-        }
-
-    };
-
-    template<typename FreqTag, typename FloatType, size_t Length>
-    class vectorx_obj : public vectorx<FloatType, Length>,
-                        public small_obj<vectorx_obj<FreqTag, FloatType, Length>> {
-    public:
-        vectorx_obj() = default;
-
-        template<typename OtherFloatType>
-        explicit vectorx_obj(const OtherFloatType(&list)[Length])
-                :vectorx<FloatType, Length>(list) {}
-
-        template<typename OtherFloatPointerType>
-        explicit vectorx_obj(const vectorx_view<OtherFloatPointerType, Length> &other)
-                :vectorx<FloatType, Length>(other) {}
-
-        template<typename OtherFloatType>
-        explicit vectorx_obj(const vectorx<OtherFloatType, Length> &other)
-                :vectorx<FloatType, Length>(other) {}
-
-        template<typename OtherFloatType, size_t OtherRows, size_t OtherColumns>
-        explicit vectorx_obj(const Eigen::Matrix<OtherFloatType, OtherRows, OtherColumns> &other)
-                :vectorx<FloatType, Length>(other) {}
-    };
-
-    using vector3d = vectorx<double, 3>;
-    using vector6d = vectorx<double, 6>;
-
-    using vector3d_cview = vectorx_view<const double *, 3>;
-    using vector6d_cview = vectorx_view<const double *, 6>;
-
-    template<typename FreqTag>
-    using vector3d_obj = vectorx_obj<FreqTag, double, 3>;
-    template<typename FreqTag>
-    using vector6d_obj = vectorx_obj<FreqTag, double, 6>;
-
-}
-
-#endif //SOPHIAR2_VECTORX_HTTP

src/core/types/versatile_data.hpp

@@ -0,0 +1,137 @@
+#ifndef SOPHIAR2_VERSATILE_DATA_HPP
+#define SOPHIAR2_VERSATILE_DATA_HPP
+
+#include "core/small_obj.hpp"
+#include "core/timestamp_helper.hpp"
+
+#include <Eigen/Core>
+
+#include <type_traits>
+
+namespace Eigen {
+    using Vector6d = Vector<double, 6>;
+    using Vector7d = Vector<double, 7>;
+}
+
+namespace sophiar {
+
+    bool _is_compatible(uint32_t given_type, uint32_t requested_type);
+
+    struct versatile_data {
+
+        static constexpr auto FLOAT_FILED_LENGTH = 12;
+        static constexpr auto EXTRA_FILED_LENGTH = 20;
+
+        double floats[FLOAT_FILED_LENGTH];
+        timestamp_type timestamp;
+        uint8_t extra_data[EXTRA_FILED_LENGTH];
+        uint32_t data_type;
+
+        void make_timestamp_current() {
+            timestamp = current_timestamp();
+        }
+
+        double &operator[](size_t index) {
+            assert(index < FLOAT_FILED_LENGTH);
+            return floats[index];
+        }
+
+        double operator[](size_t index) const {
+            assert(index < FLOAT_FILED_LENGTH);
+            return floats[index];
+        }
+
+        template<uint16_t Rows, uint16_t Columns, uint16_t Offset = 0>
+        auto view_as_matrix() {
+            static_assert(Offset + Rows * Columns <= FLOAT_FILED_LENGTH);
+            return Eigen::Map<typename Eigen::Matrix<double, Rows, Columns>>(floats + Offset);
+        }
+
+        template<uint16_t Rows, uint16_t Columns, uint16_t Offset = 0>
+        auto view_as_matrix() const {
+            static_assert(Offset + Rows * Columns <= FLOAT_FILED_LENGTH);
+            return Eigen::Map<const typename Eigen::Matrix<double, Rows, Columns>>(floats + Offset);
+        }
+
+        template<typename Derived>
+        versatile_data &operator>>(Derived &data) {
+            assert(_is_compatible(this->data_type, Derived::type_id));
+            data.fill_from_versatile_data(*this);
+            return *this;
+        }
+
+        template<typename Derived>
+        versatile_data &operator<<(const Derived &data) {
+            this->data_type = Derived::type_id;
+            data.write_to_versatile_data(*this);
+            return *this;
+        }
+
+    };
+
+    static_assert(sizeof(versatile_data) == 128);
+    static_assert(std::is_trivial_v<versatile_data>);
+
+    template<typename FreqTag>
+    struct versatile_obj : public versatile_data,
+                           public small_obj<versatile_obj<FreqTag>> {
+    };
+
+    enum class versatile_type_id : uint32_t {
+        COORDINATE_POSE,
+        COORDINATE_POSE_COMPACT, // 平移 + 四元数
+        COORDINATE_TWIST,
+        POINT,
+        POINT_VELOCITY,
+        FORCE_TORQUE_XYZ, // 六轴力力矩
+        ANGLE_ARR6,
+        ANGLE_ARR7,
+    };
+
+// https://eigen.tuxfamily.org/dox/TopicCustomizing_InheritingMatrix.html
+#define EIGEN_DERIVED(class_name, base_name) \
+    class_name() : \
+            base_name() {} \
+    template<typename OtherDerived> \
+    explicit class_name(const Eigen::MatrixBase<OtherDerived> &other) \
+            :base_name(other) {} \
+    template<typename OtherDerived> \
+    class_name &operator=(const Eigen::MatrixBase<OtherDerived> &other) { \
+        base_name::operator=(other); \
+        return *this; \
+    } \
+    base_name &view_as_base() { \
+        return *this; \
+    } \
+    const base_name &view_as_base() const { \
+        return *this; \
+    }
+
+    template<versatile_type_id TypeId, uint16_t Rows, uint16_t Cols>
+    struct default_versatile_wrapper : public Eigen::Matrix<double, Rows, Cols> {
+        static_assert(Rows * Cols <= versatile_data::FLOAT_FILED_LENGTH);
+
+        using base_type = Eigen::Matrix<double, Rows, Cols>;
+        EIGEN_DERIVED(default_versatile_wrapper, base_type);
+
+        static constexpr auto type_id = (uint32_t) TypeId;
+
+    protected:
+
+        void fill_from_versatile_data(const versatile_data &data) {
+            this->view_as_base() = data.view_as_matrix<Rows, Cols>();
+        }
+
+        void write_to_versatile_data(versatile_data &data) const {
+            data.view_as_matrix<Rows, Cols>() = this->view_as_base();
+        }
+
+    private:
+
+        friend class versatile_data;
+
+    };
+
+}
+
+#endif //SOPHIAR2_VERSATILE_DATA_HPP

src/extern_defs/core/types/versatile_data.cpp

@@ -0,0 +1,13 @@
+#include "core/types/versatile_data.hpp"
+
+namespace sophiar {
+
+    bool _is_compatible(uint32_t given_type, uint32_t requested_type) {
+        if (given_type == requested_type) return true;
+        switch (given_type) {
+
+        }
+        return false;
+    }
+
+}

src/main.cpp

@@ -4,16 +4,24 @@
 
 #include "robot/ur/ur_interface.h"
 #include "utility/debug_utility.hpp"
+#include "core/types/geometry_types.hpp"
 
 #include <boost/asio/co_spawn.hpp>
 #include <boost/asio/detached.hpp>
 #include <boost/uuid/uuid_generators.hpp>
 
+#include <Eigen/Geometry>
+
 using boost::asio::co_spawn;
 using boost::asio::detached;
 
 using namespace sophiar;
 
+template<typename Derived>
+auto test_func(const Eigen::MatrixBase<Derived> &m) {
+    return sizeof(std::remove_cv<decltype(m)>);
+}
+
 struct fake_state_listener : public ur_status_signal_type::listener_type {
     void on_signal_received(ur_status::pointer status) override {
         ++cnt;
@@ -23,25 +31,28 @@ struct fake_state_listener : public ur_status_signal_type::listener_type {
 };
 
 int main() {
-    ur_interface ur;
-    fake_state_listener listener;
-    ur.set_ur_ip(boost::asio::ip::make_address_v4("192.168.38.141"));
-    ur.add_status_listener(listener);
-    ur.set_report_frequency(125);
-    auto func = [&]() -> boost::asio::awaitable<void> {
-        auto &cmd_listener = ur.get_command_listener(boost::uuids::random_generator()());
-        co_await ur.init();
-        co_await ur.start();
-        for (int i = 0; i < 125; ++i) {
-            auto move = ur_command::new_tcp_speed({0, 0, 0.01, .0, .0, .0});
-            cmd_listener.on_signal_received(std::move(move));
-            co_await coro_sleep(std::chrono::milliseconds(8));
-        }
-        co_await ur.reset();
-        co_return;
-    };
-    co_spawn(ur.get_context(), func(), detached);
-    ur.get_context().run();
-    std::cout << listener.cnt << std::endl;
+//    ur_interface ur;
+//    fake_state_listener listener;
+//    ur.set_ur_ip(boost::asio::ip::make_address_v4("192.168.38.141"));
+//    ur.add_status_listener(listener);
+//    ur.set_report_frequency(125);
+//    auto func = [&]() -> boost::asio::awaitable<void> {
+//        auto &cmd_listener = ur.get_command_listener(0);
+//        co_await ur.init();
+//        co_await ur.start();
+//        for (int i = 0; i < 125; ++i) {
+//            auto move = ur_command::new_tcp_speed({0, 0, 0, 0.01, 0, 0});
+//            cmd_listener.on_signal_received(std::move(move));
+//            co_await coro_sleep(std::chrono::milliseconds(8));
+//        }
+//        co_await ur.reset();
+//        co_return;
+//    };
+//    co_spawn(ur.get_context(), func(), detached);
+//    ur.get_context().run();
+//    std::cout << listener.cnt << std::endl;
+
+    auto trans = to_matrix_obj<high_freq_tag>(Eigen::Isometry3d().matrix());
+
     return 0;
 }

src/robot/ur/ur_interface.cpp

@@ -39,6 +39,7 @@ namespace sophiar {
 
     using namespace boost::asio::experimental::awaitable_operators;
     using namespace boost::asio::ip;
+    using namespace Eigen;
 
     struct ur_interface::impl {
 
@@ -211,7 +212,7 @@ namespace sophiar {
 
             static constexpr uint16_t length() {
                 return sizeof(int32_t) * 2 +
-                       vector6d::length() * 2 +
+                       sizeof(Vector6d) * 2 +
                        sizeof(double) * 2;
             }
 
@@ -287,7 +288,7 @@ namespace sophiar {
 
         ip_address_type ur_ip;
         scoped_ptr<tcp::socket> ur_socket; // https://stackoverflow.com/questions/3062803/
-        vector6d ur_tcp{0, 0, 0, 0, 0, 0}; // TODO 让外部代码自己提供 tcp_pose
+        Vector6d ur_tcp = {0, 0, 0, 0, 0, 0}; // TODO 让外部代码自己提供 tcp_pose
 
         ur_status_signal_type ur_status_signal;
         ur_command_handler_type ur_command_handler;
@@ -582,15 +583,18 @@ namespace sophiar {
         }
 
         awaitable<void> on_stop_impl() {
+            // wait command sending stop
+            stop_requested_signal.try_notify_all();
+            co_await handle_command_request_finished_signal.coro_wait(*handle_command_request_finished_token);
+            // stop sending status
             versatile_buffer buffer;
             try {
                 co_await pause_report(buffer);
             } catch (std::exception &e) {
                 SPDLOG_ERROR("Failed to stop UR interface: {}", e.what());
             }
-            stop_requested_signal.try_notify_all();
-            co_await (handle_packages_finished_signal.coro_wait(*handle_packages_finished_token) &&
-                      handle_command_request_finished_signal.coro_wait(*handle_command_request_finished_token));
+            // wait package handler stop
+            co_await handle_packages_finished_signal.coro_wait(*handle_packages_finished_token);
             co_return;
         }
 
@@ -615,7 +619,7 @@ namespace sophiar {
         pimpl->report_frequency = freq;
     }
 
-    void ur_interface::set_tcp(const vector6d &tcp) {
+    void ur_interface::set_tcp(const Vector6d &tcp) {
         pimpl->ur_tcp = tcp;
     }
 
@@ -623,7 +627,7 @@ namespace sophiar {
         pimpl->ur_status_signal.add_listener(node);
     }
 
-    ur_command_listener_type &ur_interface::get_command_listener(const switchable_muxer::uuid_type &eid) {
+    ur_command_listener_type &ur_interface::get_command_listener(switchable_muxer::eid_type eid) {
         return pimpl->ur_command_muxer.get_listener(eid);
     }
 

src/robot/ur/ur_interface.h

@@ -4,12 +4,13 @@
 #include "core/global_io_context.hpp"
 #include "core/small_obj.hpp"
 #include "core/tristate_obj.h"
-#include "core/types/vectorx.hpp"
+#include "core/types/versatile_data.hpp"
 #include "utility/signal_muxer.hpp"
 #include "utility/tiny_signal.hpp"
 
+#include <Eigen/Core>
+
 #include <boost/asio/ip/address.hpp>
-#include <boost/uuid/uuid.hpp>
 
 #include <memory>
 
@@ -57,10 +58,10 @@ namespace sophiar {
     }
 
     struct ur_status_content {
-        vector6d target_q, target_qd, target_qdd;
-        vector6d actual_q, actual_qd;
-        vector6d actual_TCP_pose, actual_TCP_speed;
-        vector6d target_TCP_pose, target_TCP_speed;
+        Eigen::Vector6d target_q, target_qd, target_qdd;
+        Eigen::Vector6d actual_q, actual_qd;
+        Eigen::Vector6d actual_TCP_pose, actual_TCP_speed;
+        Eigen::Vector6d target_TCP_pose, target_TCP_speed;
         double timestamp;
         int32_t robot_mode;
         uint32_t runtime_state;
@@ -76,7 +77,7 @@ namespace sophiar {
     // tcp_pose and cmd_id will be filled by ur_interface
     struct ur_command_content {
         int32_t move_type, cmd_id;
-        vector6d tcp_pose, target;
+        Eigen::Vector6d tcp_pose, target;
         double param_a, param_v;
     };
 
@@ -96,7 +97,7 @@ namespace sophiar {
             return ret;
         }
 
-        static pointer new_joint_angle(const vector6d &angle,
+        static pointer new_joint_angle(const Eigen::Vector6d &angle,
                                        double ja = a_joint_default,
                                        double jv = v_joint_default) {
             auto ret = new_instance();
@@ -107,7 +108,7 @@ namespace sophiar {
             return ret;
         }
 
-        static pointer new_joint_speed(const vector6d &speed,
+        static pointer new_joint_speed(const Eigen::Vector6d &speed,
                                        double ja = a_joint_default) {
             auto ret = new_instance();
             ret->move_type = 3;
@@ -117,7 +118,7 @@ namespace sophiar {
             return ret;
         }
 
-        static pointer new_tool_pose(const vector6d &pose,
+        static pointer new_tool_pose(const Eigen::Vector6d &pose,
                                      double ta = a_tool_default,
                                      double tv = v_tool_default) {
             auto ret = new_instance();
@@ -128,7 +129,7 @@ namespace sophiar {
             return ret;
         }
 
-        static pointer new_tcp_speed(const vector6d &speed,
+        static pointer new_tcp_speed(const Eigen::Vector6d &speed,
                                      double ta = a_tool_default) {
             auto ret = new_instance();
             ret->move_type = 4;
@@ -160,11 +161,11 @@ namespace sophiar {
 
         void add_status_listener(ur_status_listener_type &node);
 
-        ur_command_listener_type &get_command_listener(const switchable_muxer::uuid_type &eid);
+        ur_command_listener_type &get_command_listener(switchable_muxer::eid_type eid);
 
         switchable_muxer *get_signal_switcher(uint8_t sid); // signal id
 
-        void set_tcp(const vector6d &tcp); // TODO 让外部提供
+        void set_tcp(const Eigen::Vector6d &tcp); // TODO 让外部提供
 
     protected:
 

src/utility/signal_muxer.hpp

@@ -3,9 +3,6 @@
 
 #include "utility/tiny_signal.hpp"
 
-#include <boost/uuid/uuid.hpp>
-#include <boost/container_hash/hash.hpp>
-
 #include <typeindex>
 #include <unordered_map>
 
@@ -13,9 +10,9 @@ namespace sophiar {
 
     // 只做引用，所以不定义虚的析构函数
     struct switchable_muxer {
-        using uuid_type = boost::uuids::uuid;
+        using eid_type = uint8_t;
 
-        virtual void set_switch_state(const uuid_type &eid, bool is_enabled) = 0;
+        virtual void set_switch_state(eid_type eid, bool is_enabled) = 0;
     };
 
     template<typename... Args>
@@ -47,7 +44,7 @@ namespace sophiar {
         explicit signal_muxer(listener_type &target)
                 : listener(target) {}
 
-        listener_type &get_listener(const uuid_type &eid) {
+        listener_type &get_listener(eid_type eid) {
             if (!listener_map.contains(eid)) {
                 listener_map.emplace(std::piecewise_construct,
                                      std::forward_as_tuple(eid),
@@ -56,14 +53,14 @@ namespace sophiar {
             return listener_map[eid];
         }
 
-        void set_switch_state(const uuid_type &eid, bool is_enabled) override {
+        void set_switch_state(eid_type eid, bool is_enabled) override {
             assert(listener_map.contains(eid));
             listener_map[eid].is_enabled = is_enabled;
         }
 
     private:
         listener_type &listener;
-        std::unordered_map<boost::uuids::uuid, proxy_listener, boost::hash<uuid_type>> listener_map;
+        std::unordered_map<eid_type, proxy_listener> listener_map;
 
     };
 

src/utility/versatile_buffer.hpp

@@ -8,6 +8,8 @@
 #include <boost/asio/use_awaitable.hpp>
 #include <boost/asio/write.hpp>
 
+#include <Eigen/Core>
+
 #include <cassert>
 #include <string>
 #include <type_traits>
@@ -31,6 +33,19 @@ namespace sophiar {
             end_pos += sizeof(T);
         }
 
+        void write_value(const std::string &str) {
+            assert(end_pos + str.length() <= VERSATILE_BUFFER_MAX_SIZE);
+            std::memcpy(buffer + end_pos, str.data(), str.length());
+            end_pos += str.length();
+        }
+
+        template<typename Derived>
+        void write_value(const Eigen::MatrixBase<Derived> &matrix) {
+            for (Eigen::Index i = 0; i < matrix.size(); ++i) {
+                write_value(matrix(i));
+            }
+        }
+
         template<typename T>
         std::enable_if_t<std::is_void_v<
                 decltype(std::declval<const T>().write_to_buffer(
@@ -39,12 +54,6 @@ namespace sophiar {
             val.write_to_buffer(*this);
         }
 
-        void write_value(const std::string &str) {
-            assert(end_pos + str.length() <= VERSATILE_BUFFER_MAX_SIZE);
-            std::memcpy(buffer + end_pos, str.data(), str.length());
-            end_pos += str.length();
-        }
-
         template<typename T>
         versatile_buffer &operator<<(const T &val) {
             write_value(val);
@@ -70,6 +79,13 @@ namespace sophiar {
             val = read_value<T>();
         }
 
+        template<typename Derived>
+        void read_value(Eigen::MatrixBase<Derived> &matrix) {
+            for (Eigen::Index i = 0; i < matrix.size(); ++i) {
+                read_value(matrix(i));
+            }
+        }
+
         template<typename T>
         std::enable_if_t<std::is_void_v<
                 decltype(std::declval<T>().fill_from_buffer(

tests/CMakeLists.txt

@@ -4,9 +4,9 @@ include_directories (${Boost_INCLUDE_DIRS})
 
 add_executable (test_core
         core/datanode_base.cpp
+        core/geometry_types.cpp
         core/small_obj.cpp
         core/tristate_obj.cpp
-        core/vectorx.cpp
         ${EXTERN_DEF_FILES}
         ${CORE_IMPL_FILES})
 target_link_libraries (test_core ${Boost_LIBRARIES} ${EXTRA_LIBS})

tests/core/datanode_base.cpp

@@ -119,7 +119,6 @@ awaitable<void> test_datanode_base_2() {
     co_await node_a.start();
     co_await node_b.start();
     auto packet = test_type::data_packet::new_instance();
-    packet->refresh();
     (*packet)[0] = 101;
     node_a.update_input_data(0, packet);
     BOOST_TEST(node_a.cnt == 101);
@@ -134,10 +133,10 @@ awaitable<void> test_datanode_base_speed(size_t length, size_t repeat) {
             auto input_data = get_input_data(0);
             auto output_data = data_packet::new_instance();
             output_data->copy_content(*input_data);
-            for (size_t i = 0; i < test_type::data_packet::DATA_PACKET_LENGTH; ++i) {
+            for (size_t i = 0; i < test_type::data_packet::FLOAT_FILED_LENGTH; ++i) {
                 (*output_data)[i] += i;
             }
-            std::reverse(output_data->data, output_data->data + test_type::data_packet::DATA_PACKET_LENGTH);
+            std::reverse(output_data->floats, output_data->floats + test_type::data_packet::FLOAT_FILED_LENGTH);
             output_data->timestamp = current_timestamp();
             cnt = (*output_data)[0];
             set_output_data(0, output_data);
@@ -158,8 +157,6 @@ awaitable<void> test_datanode_base_speed(size_t length, size_t repeat) {
     }
 
     auto test_data = test_type::data_packet::new_instance();
-    test_data->refresh();
-
     auto start_ts = current_timestamp();
     for (int i = 0; i < repeat; ++i) {
         pool[0].update_input_data(0, test_data);

tests/core/geometry_types.cpp

@@ -0,0 +1,48 @@
+#define BOOST_TEST_DYN_LINK
+
+#include <boost/test/unit_test.hpp>
+
+#include "core/types/geometry_types.hpp"
+
+using namespace sophiar;
+
+BOOST_AUTO_TEST_CASE(test_geometry_types) {
+
+    versatile_data test_data;
+
+    coordinate_pose pose;
+    pose.view_as_base() = Eigen::Translation3d(10, 20, 30) *
+                          Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitX());
+    test_data << pose;
+    coordinate_pose other_pose;
+    other_pose.setIdentity();
+    test_data >> other_pose;
+    BOOST_TEST(pose.isApprox(other_pose));
+
+    coordinate_pose_compact compact_pose;
+    compact_pose.view_as_base() = Eigen::Translation3d(10, 20, 30) *
+                                  Eigen::AngleAxisd(0.7, Eigen::Vector3d::UnitX());
+    test_data << compact_pose;
+    coordinate_pose_compact other_compact_pose;
+    other_compact_pose.setIdentity();
+    test_data >> other_compact_pose;
+    BOOST_TEST(compact_pose.isApprox(other_compact_pose));
+
+    coordinate_twist twist;
+    twist.angular_part() = Eigen::Vector3d{1, 2, 3};
+    twist.linear_part() = Eigen::Vector3d{4, 5, 6};
+    test_data << twist;
+    coordinate_twist other_twist;
+    other_twist.setZero();
+    test_data >> other_twist;
+    BOOST_TEST(twist.isApprox(other_twist));
+
+    auto some_twist = coordinate_twist::from_rot_axis({0, 0, 1}, {1, 0, 0});
+    BOOST_TEST(some_twist.isApprox(Eigen::Vector<double, 6>{0, 0, 1, 0, -1, 0}));
+    some_twist = coordinate_twist::from_rot_axis({0, 1, 0}, {1, 0, 0});
+    BOOST_TEST(some_twist.isApprox(Eigen::Vector<double, 6>{0, 1, 0, 0, 0, 1}));
+
+    Eigen::Isometry3d some_trans(Eigen::Translation3d(-1, 0, 0));
+    auto new_twist = some_trans * some_twist;
+    BOOST_TEST(new_twist.isApprox(Eigen::Vector<double, 6>{0, 1, 0, 0, 0, 0}));
+}

tests/core/vectorx.cpp

@@ -1,149 +0,0 @@
-#define BOOST_TEST_DYN_LINK
-
-#include <boost/test/unit_test.hpp>
-
-#include "core/global_io_context.hpp"
-#include "core/types/vectorx.hpp"
-
-using namespace sophiar;
-
-void test_vectorx_view_func_1(vectorx_view<double *, 2> data) {}
-
-void test_vectorx_view_func_2(vectorx_view<const double *, 2> data) {}
-
-BOOST_AUTO_TEST_CASE(test_vectorx) {
-
-    auto test_a = vector3d({3, 2, 1});
-    BOOST_TEST(test_a[0] == 3);
-    BOOST_TEST(test_a[1] == 2);
-    BOOST_TEST(test_a[2] == 1);
-    BOOST_TEST(test_a.at<0>() == 3);
-    BOOST_TEST(test_a.at<1>() == 2);
-    BOOST_TEST(test_a.at<2>() == 1);
-
-    test_a = {4, 5, 6};
-    BOOST_TEST(test_a[0] == 4);
-    BOOST_TEST(test_a[1] == 5);
-    BOOST_TEST(test_a[2] == 6);
-
-    BOOST_TEST(test_a.length() == sizeof(double) * 3);
-
-    auto view_a = test_a.get_view<0, 2>();
-    BOOST_TEST(view_a[0] == 4);
-    BOOST_TEST(view_a[1] == 5);
-    BOOST_TEST(view_a.at<0>() == 4);
-    BOOST_TEST(view_a.at<1>() == 5);
-
-    auto view_b = test_a.get_view<1, 3>();
-    BOOST_TEST(view_b[0] == 5);
-    BOOST_TEST(view_b[1] == 6);
-    BOOST_TEST(view_b.at<0>() == 5);
-    BOOST_TEST(view_b.at<1>() == 6);
-
-    view_b.at<1>() = 7;
-    BOOST_TEST(test_a.at<2>() == 7);
-
-    auto test_b = view_b.to_vectorx();
-    static_assert(std::is_same_v<decltype(test_b), vectorx<double, 2>>);
-    test_b.at<1>() = 8;
-    BOOST_TEST(view_b.at<1>() == 7);
-
-    const vector3d &const_test_a = test_a;
-    auto view_c = const_test_a.get_view<0, 2>();
-    BOOST_TEST(view_c[0] == 4);
-    BOOST_TEST(view_c[1] == 5);
-    BOOST_TEST(view_c.at<0>() == 4);
-    BOOST_TEST(view_c.at<1>() == 5);
-
-    auto test_c = view_c.to_vectorx();
-    static_assert(std::is_same_v<decltype(test_c), vectorx<double, 2>>);
-    test_c.at<1>() = 6;
-    BOOST_TEST(view_c.at<1>() == 5);
-
-    auto test_c_obj = view_c.to_vectorx_obj<high_freq_tag>();
-    BOOST_TEST(test_c_obj->at<0>() == 4);
-    BOOST_TEST(test_c_obj->at<1>() == 5);
-    test_c.at<1>() = 7;
-    BOOST_TEST(test_c_obj->at<0>() == 4);
-    BOOST_TEST(test_c_obj->at<1>() == 5);
-
-    const auto &test_d = test_c;
-
-    test_vectorx_view_func_1(view_b);
-//    test_vectorx_view_func_1(view_c);
-    test_vectorx_view_func_2(view_b);
-    test_vectorx_view_func_2(view_c);
-
-    test_vectorx_view_func_1(test_c);
-//    test_vectorx_view_func_1(test_d);
-    test_vectorx_view_func_2(test_c);
-    test_vectorx_view_func_2(test_d);
-
-    test_vectorx_view_func_1(test_c.get_view());
-//    test_vectorx_view_func_1(test_d.get_view());
-    test_vectorx_view_func_2(test_c.get_view());
-    test_vectorx_view_func_2(test_d.get_view());
-
-    test_a = {1, 2, 3};
-    auto eigen_column = test_a.get_view().as_eigen_column_view();
-    BOOST_TEST(eigen_column(0) == 1);
-    BOOST_TEST(eigen_column(1) == 2);
-    BOOST_TEST(eigen_column(2) == 3);
-
-    auto transposed = eigen_column.transpose();
-    transposed(1) = 3; // will affect origin
-    BOOST_TEST(eigen_column(1) == 3);
-    BOOST_TEST(test_a.at<1>() == 3);
-
-    Eigen::Vector3d real_vector = eigen_column;
-    real_vector(0) = 2; // will not affect origin
-    BOOST_TEST(eigen_column(0) == 1);
-    BOOST_TEST(test_a.at<0>() == 1);
-
-    auto eigen_const_column = test_a.get_view().as_eigen_const_column_view();
-    test_a = {1, 2, 3};
-    BOOST_TEST(eigen_const_column(0) == 1);
-    BOOST_TEST(eigen_const_column(1) == 2);
-    BOOST_TEST(eigen_const_column(2) == 3);
-
-    Eigen::Vector3d real_vector_2 = eigen_const_column;
-    real_vector(0) = 2; // will not affect origin
-    BOOST_TEST(eigen_const_column(0) == 1);
-    BOOST_TEST(test_a.at<0>() == 1);
-
-    test_a.get_view().fill({10}).fill({11}).fill({12});
-    BOOST_TEST(test_a[0] == 10);
-    BOOST_TEST(test_a[1] == 11);
-    BOOST_TEST(test_a[2] == 12);
-
-    test_a.get_view().fill({13, 14}).fill({15});
-    BOOST_TEST(test_a[0] == 13);
-    BOOST_TEST(test_a[1] == 14);
-    BOOST_TEST(test_a[2] == 15);
-
-    auto test_e = vectorx<double, 1>({16});
-    auto test_f = vectorx<double, 2>({17, 18});
-    test_a.get_view().fill(test_e).fill(test_f);
-    BOOST_TEST(test_a[0] == 16);
-    BOOST_TEST(test_a[1] == 17);
-    BOOST_TEST(test_a[2] == 18);
-
-    test_a = {1, 2, 3};
-    test_a.get_view().fill(test_e.get_view()).fill(test_f.get_view());
-    BOOST_TEST(test_a[0] == 16);
-    BOOST_TEST(test_a[1] == 17);
-    BOOST_TEST(test_a[2] == 18);
-
-    auto eigen_vec3 = Eigen::Vector3d(20, 21, 22);
-    test_a = eigen_vec3;
-    BOOST_TEST(test_a[0] == 20);
-    BOOST_TEST(test_a[1] == 21);
-    BOOST_TEST(test_a[2] == 22);
-
-    test_a = {1, 2, 3};
-    test_a.get_view().fill(eigen_vec3);
-    BOOST_TEST(test_a[0] == 20);
-    BOOST_TEST(test_a[1] == 21);
-    BOOST_TEST(test_a[2] == 22);
-
-}