feat: add EtherCAT DC monitoring and four-motor sync test

CMakeLists.txt

@@ -18,7 +18,9 @@ set(CMAKE_INSTALL_PREFIX "${CMAKE_SOURCE_DIR}/output" CACHE PATH "" FORCE)
 set(CMAKE_BUILD_RPATH "\$ORIGIN:\$ORIGIN/../lib")
 set(CMAKE_INSTALL_RPATH "\$ORIGIN:\$ORIGIN/../lib")
 
-# Use RUNPATH (new dtags) generally preferable
+# Use transitive RPATH so CLion can run build-tree test executables without
+# manually setting LD_LIBRARY_PATH for indirect third-party dependencies.
+add_link_options(-Wl,--disable-new-dtags)
 set(CMAKE_BUILD_WITH_INSTALL_RPATH OFF)
 set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
 

README.md

@@ -1,23 +1,25 @@
 # CMVR-ES
 
-## Overview
+## 简介
 
-## Installation
+CMVR-ES 工程。
 
-### 1. Git submodules install
+## 安装
+
+### 1. 拉取 Git 子模块
 
 ```
 git submodule update --init --recursive
 ```
 
-### 2. Dependency install
+### 2. 安装系统依赖
 
 ```shell
-# basic
+# 基础工具
 sudo apt-get update
 sudo apt install -y build-essential cmake git pkg-config patchelf libboost-all-dev libssl-dev
 
-# opencv
+# OpenCV
 sudo apt install -y \
   libjpeg-dev libpng-dev libtiff-dev \
   libavcodec-dev libavformat-dev libswscale-dev \
@@ -39,7 +41,7 @@ sudo apt-get install libassimp-dev
 # visp
 sudo apt-get install -y libx11-dev liblapack-dev libzbar-dev libpthread-stubs0-dev libdc1394-dev nlohmann-json3-dev
 
-# realsense
+# RealSense
 sudo apt-get install -y \
   libusb-1.0-0-dev libudev-dev \
   libglu1-mesa-dev
@@ -48,4 +50,91 @@ sudo apt-get install -y \
 sudo apt install gnuplot-qt
 ```
 
+### 3. 配置 IgH EtherCAT
 
+工程已内置 IgH EtherCAT 1.7.0 的 userspace 文件：
+
+```text
+dependency/x86/third_party/ethercat/v1.7.0
+```
+
+先设置本机路径：
+
+```shell
+export CMVR_ES_ROOT=/path/to/cmvr-es
+export IGH_ETHERCAT_ROOT=$CMVR_ES_ROOT/dependency/x86/third_party/ethercat/v1.7.0
+```
+
+安装当前内核的 header：
+
+```shell
+sudo apt-get update
+sudo apt-get install -y linux-headers-$(uname -r)
+```
+
+如果内置目录里已经有当前内核版本的 EtherCAT 内核模块，直接安装到系统：
+
+```shell
+sudo mkdir -p /lib/modules/$(uname -r)/ethercat
+sudo cp -r $IGH_ETHERCAT_ROOT/lib/modules/$(uname -r)/ethercat/* /lib/modules/$(uname -r)/ethercat/
+sudo depmod
+```
+
+内置内核模块只适用于相同内核版本。如果 `$IGH_ETHERCAT_ROOT/lib/modules/$(uname -r)` 不存在，说明这台机器的内核版本不匹配，需要在这台机器上重新编译安装 IgH EtherCAT：
+
+```shell
+sudo apt-get update
+sudo apt-get install -y \
+  build-essential autoconf automake libtool pkg-config git \
+  linux-headers-$(uname -r)
+
+cd /tmp
+git clone --branch stable-1.7 --depth 1 https://gitlab.com/etherlab.org/ethercat.git ethercat-stable-1.7
+cd ethercat-stable-1.7
+
+./bootstrap
+./configure \
+  --prefix=$IGH_ETHERCAT_ROOT \
+  --libdir=$IGH_ETHERCAT_ROOT/lib \
+  --includedir=$IGH_ETHERCAT_ROOT/include \
+  --sysconfdir=$IGH_ETHERCAT_ROOT/etc \
+  --with-systemdsystemunitdir=$IGH_ETHERCAT_ROOT/lib/systemd/system \
+  --enable-generic \
+  --with-linux-dir=/lib/modules/$(uname -r)/build
+
+make -j$(nproc) all modules
+make install
+sudo make modules_install
+sudo depmod
+```
+
+启动 EtherCAT。`eno1` 换成实际连接 EtherCAT 从站的网卡：
+
+```shell
+sudo script/ethercat/start_ethercat.sh eno1
+```
+
+脚本会写入内置 IgH 配置文件：
+
+```text
+$IGH_ETHERCAT_ROOT/etc/ethercat.conf
+```
+
+脚本会把该网卡的 MAC 写到 `MASTER0_DEVICE`，使用 `DEVICE_MODULES="generic"`，把网卡从 NetworkManager 断开，并通过 `ethercatctl -c` 启动 IgH master。
+
+查看状态：
+
+```shell
+script/ethercat/status_ethercat.sh
+
+$IGH_ETHERCAT_ROOT/bin/ethercat master
+$IGH_ETHERCAT_ROOT/bin/ethercat slaves
+$IGH_ETHERCAT_ROOT/bin/ethercat pdos
+```
+
+停止 EtherCAT：
+
+```shell
+sudo script/ethercat/stop_ethercat.sh eno1
+sudo script/ethercat/stop_ethercat.sh eno1 --restore-network
+```

cmvr-es/config/devices/motor/ethercat_motors.pb.txt

@@ -0,0 +1,65 @@
+motor {
+  id: "ethercat_motors"
+
+  motor_groups {
+    id: "right_arm_ethercat"
+    bus_type: MOTOR_BUS_ETHERCAT
+    vendor: MOTOR_VENDOR_EYOU
+    protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402
+
+    ethercat {
+      master_index: 0
+      cycle_us: 1000
+      slave_op_timeout_ms: 12000
+      slave_state_poll_period_ms: 10
+
+      cia402 {
+        profile_position_trigger_delay_ms: 2
+        state_transition_timeout_ms: 1200
+        velocity_stop_timeout_ms: 2000
+        status_poll_period_ms: 10
+        stopped_velocity_tolerance_rad_s: 0.001
+      }
+
+      dc {
+        enable: true
+        reference_motor_id: 1
+        sync0_cycle_us: 1000
+        sync0_shift_us: 0
+        sync_reference_clock_period: 1
+        assign_activate: 768
+        sync_monitor_period_ms: 1000
+      }
+
+      slaves { motor_id: 1 alias: 0 position: 0 }
+      slaves { motor_id: 2 alias: 0 position: 1 }
+      slaves { motor_id: 3 alias: 0 position: 2 }
+      slaves { motor_id: 4 alias: 0 position: 3 }
+      slaves { motor_id: 5 alias: 0 position: 4 }
+      slaves { motor_id: 6 alias: 0 position: 5 }
+      slaves { motor_id: 7 alias: 0 position: 6 }
+    }
+
+    joint_limits {
+      enable: true
+      source: JOINT_LIMIT_SOURCE_CUSTOM
+      joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
+    }
+
+    motors {
+      motors { id: 1 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 2 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 3 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 4 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 5 joint_name: "R_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 6 joint_name: "R_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 7 joint_name: "R_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+    }
+  }
+}

cmvr-es/config/devices/motor/ethercat_motors_two_real_test.pb.txt

@@ -0,0 +1,56 @@
+motor {
+  id: "ethercat_motors"
+
+  motor_groups {
+    id: "right_arm_ethercat"
+    bus_type: MOTOR_BUS_ETHERCAT
+    vendor: MOTOR_VENDOR_EYOU
+    protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402
+
+    ethercat {
+      master_index: 0
+      cycle_us: 1000
+      slave_op_timeout_ms: 15000
+      slave_state_poll_period_ms: 10
+
+      cia402 {
+        profile_position_trigger_delay_ms: 2
+        state_transition_timeout_ms: 1200
+        velocity_stop_timeout_ms: 2000
+        status_poll_period_ms: 10
+        stopped_velocity_tolerance_rad_s: 0.001
+      }
+
+      dc {
+        enable: false
+        reference_motor_id: 1
+        sync0_cycle_us: 1000
+        sync0_shift_us: 0
+        sync_reference_clock_period: 1
+        assign_activate: 768
+        sync_monitor_period_ms: 1000
+      }
+
+      slaves { motor_id: 1 alias: 0 position: 0 }
+      slaves { motor_id: 2 alias: 0 position: 1 }
+      slaves { motor_id: 3 alias: 0 position: 2 }
+      slaves { motor_id: 4 alias: 0 position: 3 }
+    }
+
+    joint_limits {
+      enable: true
+      source: JOINT_LIMIT_SOURCE_CUSTOM
+      joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
+      joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
+    }
+
+    motors {
+      motors { id: 1 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 2 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 3 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 4 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+    }
+  }
+}

cmvr-es/config/devices/motor/mujoco_motors.pb.txt

@@ -6,7 +6,6 @@ motor {
     bus_type: MOTOR_BUS_MUJOCO
     vendor: MOTOR_VENDOR_MUJOCO
     protocol: MOTOR_PROTOCOL_MUJOCO
-    tool_frame: "R_FINGER_TIP"
     mujoco {
       world_id: "mujoco_world"
     }

cmvr-es/config/devices/motor/ti5_motors.pb.txt

@@ -6,7 +6,6 @@ motor {
     bus_type: MOTOR_BUS_CAN
     vendor: MOTOR_VENDOR_TI5
     protocol: MOTOR_PROTOCOL_CANOPEN
-    tool_frame: "L_FINGER_TIP"
     can {
       channel_id: 0
     }
@@ -16,13 +15,13 @@ motor {
       urdf_path: "model/xiaoyan_description/dual_arm.urdf"
     }
     motors {
-      motors { id: 23 joint_name: "L_SHOULDER_P" }
-      motors { id: 24 joint_name: "L_SHOULDER_R" }
-      motors { id: 25 joint_name: "L_SHOULDER_Y" }
-      motors { id: 26 joint_name: "L_ELBOW_R" }
-      motors { id: 27 joint_name: "L_WRIST_P" }
-      motors { id: 28 joint_name: "L_WRIST_Y" }
-      motors { id: 29 joint_name: "L_WRIST_R" }
+      motors { id: 23 joint_name: "L_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 24 joint_name: "L_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 25 joint_name: "L_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 26 joint_name: "L_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 27 joint_name: "L_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 28 joint_name: "L_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 29 joint_name: "L_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
     }
   }
 
@@ -31,7 +30,6 @@ motor {
     bus_type: MOTOR_BUS_CAN
     vendor: MOTOR_VENDOR_TI5
     protocol: MOTOR_PROTOCOL_CANOPEN
-    tool_frame: "R_FINGER_TIP"
     can {
       channel_id: 1
     }
@@ -47,13 +45,13 @@ motor {
       joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
     }
     motors {
-      motors { id: 16 joint_name: "R_SHOULDER_P" }
-      motors { id: 17 joint_name: "R_SHOULDER_R" }
-      motors { id: 18 joint_name: "R_SHOULDER_Y" }
-      motors { id: 19 joint_name: "R_ELBOW_R" }
-      motors { id: 20 joint_name: "R_WRIST_P" }
-      motors { id: 21 joint_name: "R_WRIST_Y" }
-      motors { id: 22 joint_name: "R_WRIST_R" }
+      motors { id: 16 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 17 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 18 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 19 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 20 joint_name: "R_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 21 joint_name: "R_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 22 joint_name: "R_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
     }
   }
 
@@ -73,9 +71,9 @@ motor {
       joints { joint_name: "HEAD_R" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
     }
     motors {
-      motors { id: 32 joint_name: "HEAD_Y" }
-      motors { id: 30 joint_name: "HEAD_P" }
-      motors { id: 31 joint_name: "HEAD_R" }
+      motors { id: 32 joint_name: "HEAD_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 30 joint_name: "HEAD_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 31 joint_name: "HEAD_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
     }
   }
 
@@ -94,8 +92,8 @@ motor {
       joints { joint_name: "WAIST_P" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
     }
     motors {
-      motors { id: 4 joint_name: "WAIST_Y" }
-      motors { id: 15 joint_name: "WAIST_P" }
+      motors { id: 4 joint_name: "WAIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 15 joint_name: "WAIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
     }
   }
 }

cmvr-es/config/manager/device_manager.pb.txt

@@ -2,6 +2,7 @@ device_manager {
   name: "cmvr_es"
   version: "0.1"
   description: "cmvr edge system version 0.1"
+  init_all_motors_when_no_active_joints: true
 
   devices {
     id: "mujoco_world"
@@ -74,6 +75,13 @@ device_manager {
     enable: false
   }
 
+  devices {
+    id: "ethercat_motors"
+    type: DEVICE_TYPE_MOTOR_SYSTEM
+    config_file: "devices/motor/ethercat_motors.pb.txt"
+    enable: false
+  }
+
   devices {
     id: "right_arm"
     type: DEVICE_TYPE_ROBOT_ARM

cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h

@@ -73,7 +73,7 @@ public:
     bool isConnected() const override { return motor_manager_ != nullptr; }
     Result powerOn() override { return torqueOn(); }
     Result powerOff() override { return torqueOff(); }
-    Result brakeRelease() override { return torqueOn(); }
+    Result brakeRelease() override;
     Result shutdown() override;
     Result clearFault() override { return Result::success(); }
     Result unlockProtectiveStop() override { return Result::success(); }

cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp

@@ -196,7 +196,10 @@ Result MotorRobotArm::torqueOn()
         if (!motor) {
             return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name);
         }
-        motor->brake();
+        if (!motor->torqueOn()) {
+            return Result::failure(ArmErrorCode::CommandFailed,
+                                   "failed to torque on motor for joint: " + joint_name);
+        }
     }
     emergency_stopped_ = false;
     return Result::success();
@@ -209,7 +212,26 @@ Result MotorRobotArm::torqueOff()
         if (!motor) {
             return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name);
         }
-        motor->torqueOff();
+        if (!motor->torqueOff() || !motor->brakeRelease()) {
+            return Result::failure(ArmErrorCode::CommandFailed,
+                                   "failed to torque off motor for joint: " + joint_name);
+        }
+    }
+    return Result::success();
+}
+
+Result MotorRobotArm::brakeRelease()
+{
+    for (const auto& joint_name : joint_names_) {
+        auto motor = getMotor_(joint_name);
+        if (!motor) {
+            return Result::failure(ArmErrorCode::RobotNotReady,
+                                   "motor not found for joint: " + joint_name);
+        }
+        if (!motor->brakeRelease()) {
+            return Result::failure(ArmErrorCode::CommandFailed,
+                                   "failed to release brake for joint: " + joint_name);
+        }
     }
     return Result::success();
 }
@@ -238,7 +260,10 @@ Result MotorRobotArm::emergencyStop()
         if (!motor) {
             return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name);
         }
-        motor->brake();
+        if (!motor->quickStop()) {
+            return Result::failure(ArmErrorCode::CommandFailed,
+                                   "failed to quick stop motor for joint: " + joint_name);
+        }
     }
     emergency_stopped_ = true;
     return Result::success();
@@ -298,7 +323,11 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
         }
         for (std::size_t i = 0; i < motors.size(); ++i) {
             const double qd = i < sample.velocity.size() ? sample.velocity[i] : 0.0;
-            motors[i]->setTarget(sample.position[i], qd);
+            if (!motors[i]->commandCyclicPosition(sample.position[i], qd)) {
+                return Result::failure(ArmErrorCode::CommandFailed,
+                                       "failed to command cyclic position for joint: " +
+                                           motors[i]->jointName());
+            }
         }
         if (k + 1 < samples.size()) {
             const double next_t = samples[k + 1].t > 0.0 ? samples[k + 1].t
@@ -331,7 +360,11 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity,
             if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) {
                 motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY);
             }
-            motor->setTarget(velocity.velocity[i] * speed_scaling_);
+            if (!motor->commandCyclicVelocity(velocity.velocity[i] * speed_scaling_)) {
+                return Result::failure(ArmErrorCode::CommandFailed,
+                                       "failed to command cyclic velocity for joint: " +
+                                           joint_names_[i]);
+            }
         }
     }
 
@@ -457,7 +490,11 @@ Result MotorRobotArm::servoJ(const JointPositionCommand& target)
         if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
             motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
         }
-        motor->setTarget(target.position[i], 0.0);
+        if (!motor->commandCyclicPosition(target.position[i], 0.0)) {
+            return Result::failure(ArmErrorCode::CommandFailed,
+                                   "failed to command cyclic position for joint: " +
+                                       joint_names_[i]);
+        }
     }
     return Result::success();
 }
@@ -740,7 +777,9 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj
             return false;
         }
         for (std::size_t j = 0; j < motors.size(); ++j) {
-            motors[j]->setTarget(position[j], velocity[j]);
+            if (!motors[j]->commandCyclicPosition(position[j], velocity[j])) {
+                return false;
+            }
         }
         next_deadline += std::chrono::duration_cast<std::chrono::steady_clock::duration>(
             std::chrono::duration<double>(dt_segment));

cmvr-es/devices/canbus/canopen/sdo_request_protocol.h

@@ -30,7 +30,7 @@ namespace cmvr {
                 return BASE_ID + sdo_frame_.node_id();
             }
 
-            void SetFrameData(msgs::CommandSpecifier cs, msgs::ObIndex index,msgs::ObSubIndex sub_index, uint32_t data) {
+            void SetFrameData(msgs::CommandSpecifier cs, uint32_t index, uint32_t sub_index, uint32_t data) {
                 std::lock_guard<std::mutex> lock(mutex_);
                 sdo_frame_.set_cs(cs);
                 sdo_frame_.set_index(index);

cmvr-es/devices/canbus/canopen/sdo_response_protocol.h

@@ -49,10 +49,10 @@ namespace cmvr {
             auto command = static_cast<msgs::CommandSpecifier>(bytes[0]);
 
             // 解析 index（字节1和字节2，低字节优先）
-            auto index = static_cast<msgs::ObIndex>(bytes[1] + (bytes[2] << 8));
+            const uint32_t index = bytes[1] + (bytes[2] << 8);
 
             // 解析 subindex（字节3）
-            auto subindex = static_cast<msgs::ObSubIndex>(bytes[3]);
+            const uint32_t subindex = bytes[3];
 
             // 根据 command 解析 data（字节4~7）
             uint32_t data = 0;
@@ -94,4 +94,4 @@ namespace cmvr {
             ParseSdoData(sdo_response_, sensor_data);
         }
     }
-}
+}

cmvr-es/devices/motor/CMakeLists.txt

@@ -1,6 +1,10 @@
 add_library(motor_core INTERFACE)
 
-target_include_directories(motor_core INTERFACE ${CMAKE_SOURCE_DIR}/cmvr-es/devices)
+target_include_directories(motor_core
+        INTERFACE
+        ${CMAKE_SOURCE_DIR}/cmvr-es
+        ${CMAKE_SOURCE_DIR}/cmvr-es/devices
+)
 
 target_link_libraries(motor_core
         INTERFACE
@@ -12,4 +16,5 @@ add_library(cmvr_es::device::motor_core ALIAS motor_core)
 add_subdirectory(drivers/ti5_canopen)
 add_subdirectory(drivers/mujoco)
 add_subdirectory(bus_runtime)
+add_subdirectory(drivers/ethercat_motor)
 add_subdirectory(manager)

cmvr-es/devices/motor/abstract_motor.h

@@ -66,13 +66,22 @@ namespace cmvr::device{
             return protocol_->getMode(node_id_);
         }
 
-        virtual void torqueOff() {
+        virtual bool torqueOn() {
             std::scoped_lock lock(mtx_);
             if (!protocol_) {
                 CMVR_LOG(ERROR) << "Protocol not set for motor";
-                return;
+                return false;
             }
-            protocol_->torqueOff(node_id_);
+            return protocol_->torqueOn(node_id_);
+        }
+
+        virtual bool torqueOff() {
+            std::scoped_lock lock(mtx_);
+            if (!protocol_) {
+                CMVR_LOG(ERROR) << "Protocol not set for motor";
+                return false;
+            }
+            return protocol_->torqueOff(node_id_);
         }
 
         virtual void setLimitQ(double ub, double lb) {
@@ -101,43 +110,69 @@ namespace cmvr::device{
         }
         // virtual void setLimitTau(double tau) = 0;
         // virtual void setLimitCurrent(double tau) = 0;
-        virtual void brake() {
+        virtual bool brakeRelease() {
             std::scoped_lock lock(mtx_);
             if (!protocol_) {
                 CMVR_LOG(ERROR) << "Protocol not set for motor";
-                return;
+                return false;
             }
-            protocol_->brake(node_id_);
-        }
-        /**
-         *
-         * @param q unit : rad
-         */
-        virtual void setQ(double q) {
-            std::scoped_lock lock(mtx_);
-            if (!protocol_) {
-                CMVR_LOG(ERROR) << "Protocol not set for motor";
-                return;
-            }
-            protocol_->setQ(node_id_, q);
+            return protocol_->brakeRelease(node_id_);
         }
 
-        virtual void setTarget(double q,double qd) {
+        virtual bool quickStop() {
             std::scoped_lock lock(mtx_);
             if (!protocol_) {
                 CMVR_LOG(ERROR) << "Protocol not set for motor";
-                return;
+                return false;
             }
-            protocol_->setTarget(node_id_,q, qd);
+            return protocol_->quickStop(node_id_);
+        }
+        virtual bool commandProfilePosition(double target_q,
+                                            double max_qd = 0.0,
+                                            double max_qdd = 0.0) {
+            std::scoped_lock lock(mtx_);
+            if (!protocol_) {
+                CMVR_LOG(ERROR) << "Protocol not set for motor";
+                return false;
+            }
+            return protocol_->commandProfilePosition(node_id_, target_q, max_qd, max_qdd);
         }
 
-        virtual void setTarget(double qd) {
+        virtual bool commandProfileVelocity(double target_qd, double max_qdd = 0.0) {
             std::scoped_lock lock(mtx_);
             if (!protocol_) {
                 CMVR_LOG(ERROR) << "Protocol not set for motor";
-                return;
+                return false;
             }
-            protocol_->setTarget(node_id_, qd);
+            return protocol_->commandProfileVelocity(node_id_, target_qd, max_qdd);
+        }
+
+        virtual bool commandCyclicPosition(double target_q,
+                                           double target_qd = 0.0) {
+            std::scoped_lock lock(mtx_);
+            if (!protocol_) {
+                CMVR_LOG(ERROR) << "Protocol not set for motor";
+                return false;
+            }
+            return protocol_->commandCyclicPosition(node_id_, target_q, target_qd);
+        }
+
+        virtual bool commandCyclicVelocity(double target_qd) {
+            std::scoped_lock lock(mtx_);
+            if (!protocol_) {
+                CMVR_LOG(ERROR) << "Protocol not set for motor";
+                return false;
+            }
+            return protocol_->commandCyclicVelocity(node_id_, target_qd);
+        }
+
+        virtual bool commandCyclicTorque(double target_tau) {
+            std::scoped_lock lock(mtx_);
+            if (!protocol_) {
+                CMVR_LOG(ERROR) << "Protocol not set for motor";
+                return false;
+            }
+            return protocol_->commandCyclicTorque(node_id_, target_tau);
         }
 
         virtual bool calibrateZeroQ() {
@@ -156,16 +191,6 @@ namespace cmvr::device{
             }
             return  protocol_->reachedTargetQ(node_id_);
         }
-        // rad /s
-        virtual void setQd(double qd) {
-            std::scoped_lock lock(mtx_);
-            if (!protocol_) {
-                CMVR_LOG(ERROR) << "Protocol not set for motor";
-                return;
-            }
-            return  protocol_->setQd(node_id_,qd);
-        }
-        // virtual void setQdd(double qdd) = 0;  // rad /s^2
         // virtual void setTau(double tau) = 0;   // N m
         // virtual void clear_err() = 0;
         // virtual void getStatus() = 0;

cmvr-es/devices/motor/bus_runtime/CMakeLists.txt

@@ -4,7 +4,18 @@ add_library(motor_bus_runtime SHARED
         ethercat/src/ethercat_motor_bus_runtime.cpp
 )
 
-target_include_directories(motor_bus_runtime PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+set(IGH_ETHERCAT_ROOT
+        ${CMAKE_SOURCE_DIR}/dependency/x86/third_party/ethercat/v1.7.0
+)
+
+target_include_directories(motor_bus_runtime
+        PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}
+        PRIVATE
+        ${IGH_ETHERCAT_ROOT}/include
+)
+
+target_link_directories(motor_bus_runtime PRIVATE ${IGH_ETHERCAT_ROOT}/lib)
 
 target_link_libraries(motor_bus_runtime
         PUBLIC
@@ -12,9 +23,28 @@ target_link_libraries(motor_bus_runtime
         cmvr_es::device::motor_core
         cmvr_es::mujoco_world
         PRIVATE
+        ethercat
         cmvr_es::device::canbus
         glog
 )
 
 add_library(cmvr_es::device::motor_bus_runtime ALIAS motor_bus_runtime)
 install(TARGETS motor_bus_runtime LIBRARY DESTINATION lib)
+
+add_executable(ethercat_motor_bus_runtime_real_test
+        ethercat/src/ethercat_motor_bus_runtime_real_test.cpp
+)
+
+target_include_directories(ethercat_motor_bus_runtime_real_test
+        PRIVATE
+        ${CMAKE_SOURCE_DIR}/cmvr-es
+)
+
+target_link_libraries(ethercat_motor_bus_runtime_real_test
+        PRIVATE
+        cmvr_es::device::motor_bus_runtime
+        gtest
+        gtest_main
+        pthread
+        glog
+)

cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h

@@ -1,10 +1,23 @@
 #ifndef CMVR_ES_ETHERCAT_MOTOR_BUS_RUNTIME_H
 #define CMVR_ES_ETHERCAT_MOTOR_BUS_RUNTIME_H
 
+#include <atomic>
+#include <chrono>
+#include <cstdint>
+#include <cstring>
+#include <mutex>
 #include <string>
+#include <thread>
+#include <type_traits>
 #include <unordered_map>
+#include <vector>
 
 #include "../../abstract_motor_bus_runtime.h"
+#include "motor/bus_runtime/ethercat/include/ethercat_pdo_mapping.h"
+
+typedef struct ec_domain ec_domain_t;
+typedef struct ec_master ec_master_t;
+typedef struct ec_slave_config ec_slave_config_t;
 
 namespace cmvr::device {
 
@@ -17,12 +30,136 @@ public:
 
     const std::string& id() const { return id_; }
     const config::EtherCATConfig& config() const { return config_; }
+    void setPdoMapping(EthercatPdoMapping mapping);
+    const EthercatPdoMapping& pdoMapping() const { return pdo_mapping_; }
     const config::EthercatSlaveConfig* slaveForMotor(int motor_id) const;
+    bool hasMotor(int motor_id) const;
+
+    bool hasPdoEntry(int motor_id, std::uint16_t index, std::uint8_t subindex) const;
+
+    template <typename T>
+    bool writePdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T value)
+    {
+        return writePdoRaw_(motor_id, index, subindex, valueBitLength_<T>(),
+                            toRawValue_(value));
+    }
+
+    template <typename T>
+    bool readPdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T& value) const
+    {
+        std::uint64_t raw = 0;
+        if (!readPdoRaw_(motor_id, index, subindex, valueBitLength_<T>(), raw)) {
+            return false;
+        }
+        value = fromRawValue_<T>(raw);
+        return true;
+    }
+
+    template <typename T>
+    bool writeSdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T value)
+    {
+        return writeSdoRaw_(motor_id, index, subindex, valueBitLength_<T>(),
+                            toRawValue_(value));
+    }
+
+    template <typename T>
+    bool readSdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T& value)
+    {
+        std::uint64_t raw = 0;
+        if (!readSdoRaw_(motor_id, index, subindex, valueBitLength_<T>(), raw)) {
+            return false;
+        }
+        value = fromRawValue_<T>(raw);
+        return true;
+    }
 
 private:
+    struct PdoEntryRuntime {
+        EthercatPdoEntryConfig cfg;
+        unsigned int offset{0};
+        bool rx{false};
+        std::uint64_t value{0};
+    };
+
+    struct SlaveRuntime {
+        config::EthercatSlaveConfig cfg;
+        ec_slave_config_t* slave_config{nullptr};
+        std::unordered_map<std::uint32_t, PdoEntryRuntime> pdo_entries;
+    };
+
+    bool configureSlave_(SlaveRuntime& slave);
+    bool configureDc_();
+    bool waitSlavesOperational_();
+    void cyclicLoop_();
+    void readFeedbackLocked_();
+    void writeCommandsLocked_();
+    void releaseMaster_();
+    bool hasValidPdoMapping_() const;
+    bool writePdoRaw_(int motor_id, std::uint16_t index, std::uint8_t subindex,
+                      std::uint8_t bit_len, std::uint64_t value);
+    bool readPdoRaw_(int motor_id, std::uint16_t index, std::uint8_t subindex,
+                     std::uint8_t bit_len, std::uint64_t& value) const;
+    bool writeSdoRaw_(int motor_id, std::uint16_t index, std::uint8_t subindex,
+                      std::uint8_t bit_len, std::uint64_t value);
+    bool readSdoRaw_(int motor_id, std::uint16_t index, std::uint8_t subindex,
+                     std::uint8_t bit_len, std::uint64_t& value);
+
+    template <typename T>
+    static constexpr std::uint8_t valueBitLength_()
+    {
+        using ValueType = std::remove_cv_t<T>;
+        static_assert(std::is_integral_v<ValueType>, "EtherCAT object values must be integral");
+        static_assert(!std::is_same_v<ValueType, bool>, "bool is not a valid EtherCAT object value");
+        static_assert(sizeof(ValueType) == 1 || sizeof(ValueType) == 2 || sizeof(ValueType) == 4,
+                      "only 8/16/32-bit EtherCAT object values are supported");
+        return static_cast<std::uint8_t>(sizeof(ValueType) * 8);
+    }
+
+    template <typename T>
+    static std::uint64_t toRawValue_(T value)
+    {
+        using ValueType = std::remove_cv_t<T>;
+        using UnsignedType = std::make_unsigned_t<ValueType>;
+        return static_cast<std::uint64_t>(static_cast<UnsignedType>(value));
+    }
+
+    template <typename T>
+    static T fromRawValue_(std::uint64_t raw)
+    {
+        using ValueType = std::remove_cv_t<T>;
+        using UnsignedType = std::make_unsigned_t<ValueType>;
+        const auto unsigned_value = static_cast<UnsignedType>(raw);
+        ValueType value{};
+        std::memcpy(&value, &unsigned_value, sizeof(ValueType));
+        return value;
+    }
+
+    static std::uint32_t pdoEntryKey_(std::uint16_t index, std::uint8_t subindex);
+    static std::string hexIndex_(std::uint32_t index);
+    static std::uint64_t maskValue_(std::uint64_t value, std::uint8_t bit_len);
+    static bool isSupportedBitLength_(std::uint8_t bit_len);
+    static std::uint64_t readEntryValue_(const std::uint8_t* domain_data,
+                                         const PdoEntryRuntime& entry);
+    static void writeEntryValue_(std::uint8_t* domain_data,
+                                 const PdoEntryRuntime& entry);
+    static std::uint64_t steadyTimeNs_();
+    static std::uint64_t timePointNs_(std::chrono::steady_clock::time_point time_point);
+    static std::uint32_t usToNs_(std::uint32_t value_us);
+    static std::int32_t usToNs_(std::int32_t value_us);
+
     std::string id_;
     config::EtherCATConfig config_;
-    std::unordered_map<int, const config::EthercatSlaveConfig*> slaves_by_motor_id_;
+    EthercatPdoMapping pdo_mapping_;
+    std::unordered_map<int, SlaveRuntime> slaves_by_motor_id_;
+
+    ec_master_t* master_{nullptr};
+    ec_domain_t* domain_{nullptr};
+    std::uint8_t* domain_data_{nullptr};
+
+    mutable std::mutex data_mutex_;
+    std::thread cyclic_thread_;
+    std::atomic<bool> running_{false};
+    bool initialized_{false};
     bool started_{false};
 };
 

cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_pdo_mapping.h

@@ -0,0 +1,35 @@
+#ifndef CMVR_ES_ETHERCAT_PDO_MAPPING_H
+#define CMVR_ES_ETHERCAT_PDO_MAPPING_H
+
+#include <cstdint>
+#include <string>
+#include <vector>
+
+namespace cmvr::device {
+
+struct EthercatPdoEntryConfig {
+    std::uint16_t index{0};
+    std::uint8_t subindex{0};
+    std::uint8_t bit_len{0};
+    std::string name;
+    bool padding{false};
+};
+
+struct EthercatPdoConfig {
+    std::uint16_t index{0};
+    std::uint8_t sync_manager{0};
+    bool rx{false};
+    std::vector<EthercatPdoEntryConfig> entries;
+};
+
+struct EthercatPdoMapping {
+    std::uint32_t vendor_id{0};
+    std::uint32_t product_code{0};
+    std::string name;
+    std::vector<EthercatPdoConfig> rx_pdos;
+    std::vector<EthercatPdoConfig> tx_pdos;
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_ETHERCAT_PDO_MAPPING_H

cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp

@@ -1,7 +1,18 @@
 #include "motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
 
+#include <algorithm>
+#include <array>
+#include <chrono>
+#include <cstddef>
+#include <map>
+#include <sstream>
+#include <thread>
+#include <utility>
+
 #include "common/base/logging/logger.h"
 
+#include <ecrt.h>
+
 namespace cmvr::device {
 
 bool EthercatMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg)
@@ -21,14 +32,34 @@ bool EthercatMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg)
     }
 
     config_ = group_cfg.ethercat();
-    if (config_.master_id().empty()) {
-        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] master_id is empty: " << id_;
-        return false;
-    }
     if (config_.cycle_us() <= 0) {
         CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] cycle_us must be positive: " << id_;
         return false;
     }
+    if (!config_.has_slave_op_timeout_ms()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing slave_op_timeout_ms: " << id_;
+        return false;
+    }
+    if (config_.slave_op_timeout_ms() == 0) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] slave_op_timeout_ms must be positive: "
+                        << id_;
+        return false;
+    }
+    if (!config_.has_slave_state_poll_period_ms()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing slave_state_poll_period_ms: "
+                        << id_;
+        return false;
+    }
+    if (config_.slave_state_poll_period_ms() == 0) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] slave_state_poll_period_ms must be positive: "
+                        << id_;
+        return false;
+    }
+    if (!hasValidPdoMapping_()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] PDO mapping is not configured: "
+                        << id_;
+        return false;
+    }
 
     slaves_by_motor_id_.clear();
     for (const auto& slave : config_.slaves()) {
@@ -36,34 +67,106 @@ bool EthercatMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg)
             CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] invalid motor_id in slave config: " << id_;
             return false;
         }
-        if (slave.slave_index() < 0) {
-            CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] invalid slave_index for motor "
-                            << slave.motor_id() << " in group: " << id_;
-            return false;
-        }
         if (slaves_by_motor_id_.count(slave.motor_id()) > 0) {
             CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] duplicate slave motor_id: "
                             << slave.motor_id() << " in group: " << id_;
             return false;
         }
-        slaves_by_motor_id_[slave.motor_id()] = &slave;
+        SlaveRuntime runtime;
+        runtime.cfg = slave;
+        slaves_by_motor_id_.emplace(slave.motor_id(), std::move(runtime));
     }
 
+    if (slaves_by_motor_id_.empty()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] no EtherCAT slaves configured: " << id_;
+        return false;
+    }
+
+    master_ = ecrt_request_master(config_.master_index());
+    if (!master_) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to request EtherCAT master "
+                        << config_.master_index() << ": " << id_;
+        return false;
+    }
+
+    domain_ = ecrt_master_create_domain(master_);
+    if (!domain_) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to create EtherCAT domain: " << id_;
+        releaseMaster_();
+        return false;
+    }
+
+    for (auto& [motor_id, slave] : slaves_by_motor_id_) {
+        (void)motor_id;
+        if (!configureSlave_(slave)) {
+            releaseMaster_();
+            return false;
+        }
+    }
+
+    if (!configureDc_()) {
+        releaseMaster_();
+        return false;
+    }
+
+    initialized_ = true;
     return true;
 }
 
+void EthercatMotorBusRuntime::setPdoMapping(EthercatPdoMapping mapping)
+{
+    pdo_mapping_ = std::move(mapping);
+}
+
 bool EthercatMotorBusRuntime::start()
 {
     if (started_) {
         return true;
     }
-    CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] EtherCAT master is not implemented yet: " << id_;
-    return false;
+    if (!initialized_ || !master_ || !domain_) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] runtime is not initialized: " << id_;
+        return false;
+    }
+
+    if (ecrt_master_activate(master_) != 0) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to activate EtherCAT master: " << id_;
+        return false;
+    }
+
+    domain_data_ = ecrt_domain_data(domain_);
+    if (!domain_data_) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to get EtherCAT domain data: " << id_;
+        return false;
+    }
+
+    {
+        std::lock_guard<std::mutex> lock(data_mutex_);
+        writeCommandsLocked_();
+    }
+
+    running_.store(true);
+    cyclic_thread_ = std::thread(&EthercatMotorBusRuntime::cyclicLoop_, this);
+    started_ = true;
+
+    if (!waitSlavesOperational_()) {
+        stop();
+        return false;
+    }
+
+    CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] started EtherCAT runtime: " << id_;
+    return true;
 }
 
 void EthercatMotorBusRuntime::stop()
 {
+    running_.store(false);
+    if (cyclic_thread_.joinable()) {
+        cyclic_thread_.join();
+    }
     started_ = false;
+    initialized_ = false;
+    domain_data_ = nullptr;
+    releaseMaster_();
 }
 
 const config::EthercatSlaveConfig* EthercatMotorBusRuntime::slaveForMotor(const int motor_id) const
@@ -72,7 +175,780 @@ const config::EthercatSlaveConfig* EthercatMotorBusRuntime::slaveForMotor(const
     if (it == slaves_by_motor_id_.end()) {
         return nullptr;
     }
-    return it->second;
+    return &it->second.cfg;
+}
+
+bool EthercatMotorBusRuntime::hasMotor(const int motor_id) const
+{
+    return slaves_by_motor_id_.count(motor_id) > 0;
+}
+
+bool EthercatMotorBusRuntime::hasPdoEntry(const int motor_id,
+                                          const std::uint16_t index,
+                                          const std::uint8_t subindex) const
+{
+    std::lock_guard<std::mutex> lock(data_mutex_);
+    const auto slave_it = slaves_by_motor_id_.find(motor_id);
+    if (slave_it == slaves_by_motor_id_.end()) {
+        return false;
+    }
+    return slave_it->second.pdo_entries.count(pdoEntryKey_(index, subindex)) > 0;
+}
+
+bool EthercatMotorBusRuntime::configureSlave_(SlaveRuntime& slave)
+{
+    slave.slave_config = ecrt_master_slave_config(master_,
+                                                  static_cast<std::uint16_t>(slave.cfg.alias()),
+                                                  static_cast<std::uint16_t>(slave.cfg.position()),
+                                                  pdo_mapping_.vendor_id,
+                                                  pdo_mapping_.product_code);
+    if (!slave.slave_config) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to get slave config: group=" << id_
+                        << ", motor_id=" << slave.cfg.motor_id()
+                        << ", alias=" << slave.cfg.alias()
+                        << ", position=" << slave.cfg.position()
+                        << ", vendor=" << hexIndex_(pdo_mapping_.vendor_id)
+                        << ", product=" << hexIndex_(pdo_mapping_.product_code);
+        return false;
+    }
+
+    struct SyncBuild {
+        bool rx{false};
+        std::vector<std::uint16_t> pdo_indices;
+        std::vector<std::vector<ec_pdo_entry_info_t>> entry_storage;
+        std::vector<ec_pdo_info_t> pdo_infos;
+    };
+
+    auto add_pdo_to_sync_build = [](std::map<std::uint8_t, SyncBuild>& builds,
+                                    const EthercatPdoConfig& pdo) {
+        auto& build = builds[pdo.sync_manager];
+        build.rx = pdo.rx;
+        build.pdo_indices.push_back(pdo.index);
+        auto& entries = build.entry_storage.emplace_back();
+        entries.reserve(pdo.entries.size());
+        for (const auto& entry : pdo.entries) {
+            entries.push_back({entry.index, entry.subindex, entry.bit_len});
+        }
+    };
+
+    std::map<std::uint8_t, SyncBuild> sync_builds;
+    for (const auto& pdo : pdo_mapping_.rx_pdos) {
+        add_pdo_to_sync_build(sync_builds, pdo);
+    }
+    for (const auto& pdo : pdo_mapping_.tx_pdos) {
+        add_pdo_to_sync_build(sync_builds, pdo);
+    }
+
+    for (auto& [sync_manager, build] : sync_builds) {
+        (void)sync_manager;
+        build.pdo_infos.reserve(build.entry_storage.size());
+        for (std::size_t i = 0; i < build.entry_storage.size(); ++i) {
+            auto& entries = build.entry_storage[i];
+            build.pdo_infos.push_back({
+                build.pdo_indices[i],
+                static_cast<unsigned int>(entries.size()),
+                entries.data(),
+            });
+        }
+    }
+
+    std::vector<ec_sync_info_t> sync_infos;
+    sync_infos.push_back({0, EC_DIR_OUTPUT, 0, nullptr, EC_WD_DISABLE});
+    sync_infos.push_back({1, EC_DIR_INPUT, 0, nullptr, EC_WD_DISABLE});
+    for (auto& [sync_manager, build] : sync_builds) {
+        const auto direction = build.rx ? EC_DIR_OUTPUT : EC_DIR_INPUT;
+        const auto watchdog = build.rx ? EC_WD_ENABLE : EC_WD_DISABLE;
+        sync_infos.push_back({
+            sync_manager,
+            direction,
+            static_cast<unsigned int>(build.pdo_infos.size()),
+            build.pdo_infos.data(),
+            watchdog,
+        });
+    }
+    sync_infos.push_back({0xff});
+
+    if (ecrt_slave_config_pdos(slave.slave_config, EC_END, sync_infos.data()) != 0) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to configure PDOs: group=" << id_
+                        << ", motor_id=" << slave.cfg.motor_id()
+                        << ", position=" << slave.cfg.position();
+        return false;
+    }
+
+    auto register_entry = [&](const EthercatPdoEntryConfig& entry, const bool rx) -> bool {
+        if (entry.padding) {
+            return true;
+        }
+        if (!isSupportedBitLength_(entry.bit_len)) {
+            CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] unsupported PDO entry bit length: "
+                            << static_cast<int>(entry.bit_len)
+                            << ", entry=" << hexIndex_(entry.index)
+                            << ":" << static_cast<int>(entry.subindex)
+                            << ", motor_id=" << slave.cfg.motor_id()
+                            << ", group=" << id_;
+            return false;
+        }
+
+        const auto key = pdoEntryKey_(entry.index, entry.subindex);
+        if (slave.pdo_entries.count(key) > 0) {
+            CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] duplicate PDO entry: "
+                            << hexIndex_(entry.index)
+                            << ":" << static_cast<int>(entry.subindex)
+                            << ", motor_id=" << slave.cfg.motor_id()
+                            << ", group=" << id_;
+            return false;
+        }
+
+        const int result = ecrt_slave_config_reg_pdo_entry(
+            slave.slave_config, entry.index, entry.subindex, domain_, nullptr);
+        if (result < 0) {
+            CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to register PDO entry "
+                            << hexIndex_(entry.index) << ":" << static_cast<int>(entry.subindex)
+                            << ", motor_id=" << slave.cfg.motor_id()
+                            << ", group=" << id_;
+            return false;
+        }
+
+        PdoEntryRuntime runtime;
+        runtime.cfg = entry;
+        runtime.offset = static_cast<unsigned int>(result);
+        runtime.rx = rx;
+        slave.pdo_entries.emplace(key, std::move(runtime));
+        return true;
+    };
+
+    for (const auto& pdo : pdo_mapping_.rx_pdos) {
+        for (const auto& entry : pdo.entries) {
+            if (!register_entry(entry, true)) {
+                return false;
+            }
+        }
+    }
+    for (const auto& pdo : pdo_mapping_.tx_pdos) {
+        for (const auto& entry : pdo.entries) {
+            if (!register_entry(entry, false)) {
+                return false;
+            }
+        }
+    }
+
+    return true;
+}
+
+bool EthercatMotorBusRuntime::configureDc_()
+{
+    if (!config_.has_dc()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing explicit DC config: "
+                        << id_ << ". Add dc { enable: false } or a complete enabled DC config.";
+        return false;
+    }
+
+    const auto& dc = config_.dc();
+    if (!dc.has_enable()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC enable: " << id_;
+        return false;
+    }
+    if (!dc.enable()) {
+        return true;
+    }
+
+    if (!dc.has_reference_motor_id()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC reference_motor_id: "
+                        << id_;
+        return false;
+    }
+    if (!dc.has_sync0_cycle_us()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC sync0_cycle_us: "
+                        << id_;
+        return false;
+    }
+    if (!dc.has_sync0_shift_us()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC sync0_shift_us: "
+                        << id_;
+        return false;
+    }
+    if (!dc.has_sync_reference_clock_period()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC sync_reference_clock_period: "
+                        << id_;
+        return false;
+    }
+    if (!dc.has_assign_activate()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC assign_activate: "
+                        << id_;
+        return false;
+    }
+    if (!dc.has_sync_monitor_period_ms()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC sync_monitor_period_ms: "
+                        << id_;
+        return false;
+    }
+    if (dc.reference_motor_id() <= 0) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC reference_motor_id must be positive: "
+                        << id_;
+        return false;
+    }
+    if (dc.sync0_cycle_us() == 0) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC sync0_cycle_us must be positive: "
+                        << id_;
+        return false;
+    }
+    if (dc.sync_reference_clock_period() == 0) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC sync_reference_clock_period must be positive: "
+                        << id_;
+        return false;
+    }
+    if (dc.assign_activate() == 0 || dc.assign_activate() > 0xFFFFU) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC assign_activate must be in [1, 0xFFFF]: "
+                        << id_ << ", value=" << hexIndex_(dc.assign_activate());
+        return false;
+    }
+    if (dc.sync_monitor_period_ms() == 0) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC sync_monitor_period_ms must be positive: "
+                        << id_;
+        return false;
+    }
+
+    const auto reference_it = slaves_by_motor_id_.find(dc.reference_motor_id());
+    if (reference_it == slaves_by_motor_id_.end() || !reference_it->second.slave_config) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC reference motor is not configured: "
+                        << id_ << ", reference_motor_id=" << dc.reference_motor_id();
+        return false;
+    }
+
+    const int select_result = ecrt_master_select_reference_clock(
+        master_, reference_it->second.slave_config);
+    if (select_result != 0) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to select DC reference clock: "
+                        << id_ << ", reference_motor_id=" << dc.reference_motor_id()
+                        << ", result=" << select_result;
+        return false;
+    }
+
+    const auto assign_activate = static_cast<std::uint16_t>(dc.assign_activate());
+    const auto sync0_cycle_ns = usToNs_(dc.sync0_cycle_us());
+    const auto sync0_shift_ns = usToNs_(dc.sync0_shift_us());
+    for (auto& [motor_id, slave] : slaves_by_motor_id_) {
+        const int result = ecrt_slave_config_dc(slave.slave_config,
+                                                assign_activate,
+                                                sync0_cycle_ns,
+                                                sync0_shift_ns,
+                                                0,
+                                                0);
+        if (result != 0) {
+            CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to configure DC: "
+                            << id_ << ", motor_id=" << motor_id
+                            << ", position=" << slave.cfg.position()
+                            << ", result=" << result;
+            return false;
+        }
+    }
+
+    CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] configured DC: "
+                   << id_
+                   << ", reference_motor_id=" << dc.reference_motor_id()
+                   << ", sync0_cycle_ns=" << sync0_cycle_ns
+                   << ", sync0_shift_ns=" << sync0_shift_ns
+                   << ", sync_reference_clock_period=" << dc.sync_reference_clock_period()
+                   << ", assign_activate=" << hexIndex_(assign_activate)
+                   << ", sync_monitor_period_ms=" << dc.sync_monitor_period_ms();
+    return true;
+}
+
+bool EthercatMotorBusRuntime::waitSlavesOperational_()
+{
+    const auto deadline = std::chrono::steady_clock::now() +
+                          std::chrono::milliseconds(config_.slave_op_timeout_ms());
+    const auto poll_period =
+        std::chrono::milliseconds(config_.slave_state_poll_period_ms());
+
+    ec_domain_state_t last_domain_state{};
+    int last_domain_result = 0;
+    do {
+        bool all_slaves_operational = true;
+        for (const auto& [motor_id, slave] : slaves_by_motor_id_) {
+            (void)motor_id;
+            ec_slave_config_state_t state{};
+            const int result = ecrt_slave_config_state(slave.slave_config, &state);
+            if (result != 0 ||
+                !state.online ||
+                !state.operational ||
+                state.al_state != EC_AL_STATE_OP) {
+                all_slaves_operational = false;
+                break;
+            }
+        }
+
+        last_domain_result = ecrt_domain_state(domain_, &last_domain_state);
+        const bool domain_complete =
+            last_domain_result == 0 &&
+            last_domain_state.wc_state == EC_WC_COMPLETE;
+
+        if (all_slaves_operational && domain_complete) {
+            CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] all EtherCAT slaves operational: "
+                           << id_
+                           << ", working_counter=" << last_domain_state.working_counter;
+            return true;
+        }
+
+        std::this_thread::sleep_for(poll_period);
+    } while (std::chrono::steady_clock::now() < deadline);
+
+    CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] timeout waiting for EtherCAT slaves OP: "
+                    << id_
+                    << ", timeout_ms=" << config_.slave_op_timeout_ms()
+                    << ", domain_result=" << last_domain_result
+                    << ", domain_wc_state=" << static_cast<int>(last_domain_state.wc_state)
+                    << ", working_counter=" << last_domain_state.working_counter;
+
+    for (const auto& [motor_id, slave] : slaves_by_motor_id_) {
+        ec_slave_config_state_t state{};
+        const int result = ecrt_slave_config_state(slave.slave_config, &state);
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] slave state: "
+                        << id_
+                        << ", motor_id=" << motor_id
+                        << ", position=" << slave.cfg.position()
+                        << ", result=" << result
+                        << ", online=" << state.online
+                        << ", operational=" << state.operational
+                        << ", al_state=" << static_cast<int>(state.al_state);
+    }
+    return false;
+}
+
+void EthercatMotorBusRuntime::cyclicLoop_()
+{
+    const auto period = std::chrono::microseconds(config_.cycle_us());
+    const bool dc_enabled = config_.dc().enable();
+    const auto dc_sync_period = dc_enabled ? config_.dc().sync_reference_clock_period() : 0U;
+    const auto dc_monitor_period =
+        dc_enabled ? std::chrono::milliseconds(config_.dc().sync_monitor_period_ms())
+                   : std::chrono::milliseconds(0);
+    std::uint32_t dc_sync_counter = 0;
+    bool dc_monitor_queued = false;
+    auto cycle_time = std::chrono::steady_clock::now();
+    auto next_dc_monitor_time = cycle_time + dc_monitor_period;
+    while (running_.load()) {
+        if (dc_enabled) {
+            ecrt_master_application_time(master_, timePointNs_(cycle_time));
+        }
+
+        ecrt_master_receive(master_);
+        ecrt_domain_process(domain_);
+        if (dc_enabled && dc_monitor_queued) {
+            const std::uint32_t dc_sync_diff_ns =
+                ecrt_master_sync_monitor_process(master_);
+            if (dc_sync_diff_ns == static_cast<std::uint32_t>(-1)) {
+                CMVR_LOG(WARNING) << "[EthercatMotorBusRuntime] DC sync monitor failed: "
+                                  << id_;
+            } else {
+                CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] dc_sync_diff_ns="
+                               << dc_sync_diff_ns
+                               << ", group=" << id_;
+            }
+            dc_monitor_queued = false;
+        }
+        {
+            std::lock_guard<std::mutex> lock(data_mutex_);
+            readFeedbackLocked_();
+            writeCommandsLocked_();
+        }
+        ecrt_domain_queue(domain_);
+        if (dc_enabled) {
+            ++dc_sync_counter;
+            if (dc_sync_counter >= dc_sync_period) {
+                dc_sync_counter = 0;
+                ecrt_master_sync_reference_clock(master_);
+            }
+            ecrt_master_sync_slave_clocks(master_);
+
+            if (cycle_time >= next_dc_monitor_time) {
+                const int monitor_result = ecrt_master_sync_monitor_queue(master_);
+                if (monitor_result == 0) {
+                    dc_monitor_queued = true;
+                } else {
+                    CMVR_LOG(WARNING) << "[EthercatMotorBusRuntime] failed to queue DC sync monitor: "
+                                      << id_ << ", result=" << monitor_result;
+                }
+                do {
+                    next_dc_monitor_time += dc_monitor_period;
+                } while (cycle_time >= next_dc_monitor_time);
+            }
+        }
+        ecrt_master_send(master_);
+
+        cycle_time += period;
+        std::this_thread::sleep_until(cycle_time);
+    }
+}
+
+void EthercatMotorBusRuntime::readFeedbackLocked_()
+{
+    if (!domain_data_) {
+        return;
+    }
+    for (auto& [motor_id, slave] : slaves_by_motor_id_) {
+        (void)motor_id;
+        for (auto& [key, entry] : slave.pdo_entries) {
+            (void)key;
+            if (!entry.rx) {
+                entry.value = readEntryValue_(domain_data_, entry);
+            }
+        }
+    }
+}
+
+void EthercatMotorBusRuntime::writeCommandsLocked_()
+{
+    if (!domain_data_) {
+        return;
+    }
+    for (const auto& [motor_id, slave] : slaves_by_motor_id_) {
+        (void)motor_id;
+        for (const auto& [key, entry] : slave.pdo_entries) {
+            (void)key;
+            if (entry.rx) {
+                writeEntryValue_(domain_data_, entry);
+            }
+        }
+    }
+}
+
+void EthercatMotorBusRuntime::releaseMaster_()
+{
+    if (master_) {
+        ecrt_release_master(master_);
+    }
+    master_ = nullptr;
+    domain_ = nullptr;
+    domain_data_ = nullptr;
+    for (auto& [motor_id, slave] : slaves_by_motor_id_) {
+        (void)motor_id;
+        slave.slave_config = nullptr;
+        slave.pdo_entries.clear();
+    }
+}
+
+bool EthercatMotorBusRuntime::hasValidPdoMapping_() const
+{
+    return pdo_mapping_.vendor_id != 0 &&
+           pdo_mapping_.product_code != 0 &&
+           !pdo_mapping_.rx_pdos.empty() &&
+           !pdo_mapping_.tx_pdos.empty();
+}
+
+bool EthercatMotorBusRuntime::writePdoRaw_(const int motor_id,
+                                           const std::uint16_t index,
+                                           const std::uint8_t subindex,
+                                           const std::uint8_t bit_len,
+                                           const std::uint64_t value)
+{
+    std::lock_guard<std::mutex> lock(data_mutex_);
+    auto slave_it = slaves_by_motor_id_.find(motor_id);
+    if (slave_it == slaves_by_motor_id_.end()) {
+        return false;
+    }
+    auto entry_it = slave_it->second.pdo_entries.find(pdoEntryKey_(index, subindex));
+    if (entry_it == slave_it->second.pdo_entries.end()) {
+        return false;
+    }
+    auto& entry = entry_it->second;
+    if (!entry.rx || entry.cfg.bit_len != bit_len) {
+        return false;
+    }
+    entry.value = maskValue_(value, entry.cfg.bit_len);
+    if (domain_data_) {
+        writeEntryValue_(domain_data_, entry);
+    }
+    return true;
+}
+
+bool EthercatMotorBusRuntime::readPdoRaw_(const int motor_id,
+                                          const std::uint16_t index,
+                                          const std::uint8_t subindex,
+                                          const std::uint8_t bit_len,
+                                          std::uint64_t& value) const
+{
+    std::lock_guard<std::mutex> lock(data_mutex_);
+    const auto slave_it = slaves_by_motor_id_.find(motor_id);
+    if (slave_it == slaves_by_motor_id_.end()) {
+        return false;
+    }
+    const auto entry_it = slave_it->second.pdo_entries.find(pdoEntryKey_(index, subindex));
+    if (entry_it == slave_it->second.pdo_entries.end()) {
+        return false;
+    }
+    const auto& entry = entry_it->second;
+    if (entry.rx || entry.cfg.bit_len != bit_len) {
+        return false;
+    }
+    value = maskValue_(entry.value, entry.cfg.bit_len);
+    return true;
+}
+
+bool EthercatMotorBusRuntime::writeSdoRaw_(const int motor_id,
+                                           const std::uint16_t index,
+                                           const std::uint8_t subindex,
+                                           const std::uint8_t bit_len,
+                                           const std::uint64_t value)
+{
+    if (!isSupportedBitLength_(bit_len)) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] unsupported SDO write bit length: "
+                        << static_cast<int>(bit_len)
+                        << ", object=" << hexIndex_(index)
+                        << ":" << static_cast<int>(subindex)
+                        << ", motor_id=" << motor_id;
+        return false;
+    }
+
+    auto slave_it = slaves_by_motor_id_.find(motor_id);
+    if (slave_it == slaves_by_motor_id_.end()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing motor for SDO write: "
+                        << motor_id << ", object=" << hexIndex_(index)
+                        << ":" << static_cast<int>(subindex);
+        return false;
+    }
+    auto& slave = slave_it->second;
+    if (!slave.slave_config || !master_) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] runtime is not initialized for SDO write: "
+                        << id_ << ", motor_id=" << motor_id
+                        << ", object=" << hexIndex_(index)
+                        << ":" << static_cast<int>(subindex);
+        return false;
+    }
+
+    const auto raw_value = maskValue_(value, bit_len);
+    if (!started_) {
+        int result = -1;
+        switch (bit_len) {
+            case 8:
+                result = ecrt_slave_config_sdo8(slave.slave_config, index, subindex,
+                                                static_cast<std::uint8_t>(raw_value));
+                break;
+            case 16:
+                result = ecrt_slave_config_sdo16(slave.slave_config, index, subindex,
+                                                 static_cast<std::uint16_t>(raw_value));
+                break;
+            case 32:
+                result = ecrt_slave_config_sdo32(slave.slave_config, index, subindex,
+                                                 static_cast<std::uint32_t>(raw_value));
+                break;
+            default:
+                break;
+        }
+        if (result != 0) {
+            CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to configure startup SDO: "
+                            << "group=" << id_ << ", motor_id=" << motor_id
+                            << ", object=" << hexIndex_(index)
+                            << ":" << static_cast<int>(subindex)
+                            << ", bit_len=" << static_cast<int>(bit_len)
+                            << ", value=" << raw_value
+                            << ", result=" << result;
+            return false;
+        }
+        return true;
+    }
+
+    std::array<std::uint8_t, 4> data{};
+    switch (bit_len) {
+        case 8:
+            EC_WRITE_U8(data.data(), static_cast<std::uint8_t>(raw_value));
+            break;
+        case 16:
+            EC_WRITE_U16(data.data(), static_cast<std::uint16_t>(raw_value));
+            break;
+        case 32:
+            EC_WRITE_U32(data.data(), static_cast<std::uint32_t>(raw_value));
+            break;
+        default:
+            break;
+    }
+
+    const auto data_size = static_cast<std::size_t>(bit_len / 8);
+    std::uint32_t abort_code = 0;
+    const int result = ecrt_master_sdo_download(
+        master_,
+        static_cast<std::uint16_t>(slave.cfg.position()),
+        index,
+        subindex,
+        data.data(),
+        data_size,
+        &abort_code);
+    if (result != 0) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to write SDO: "
+                        << "group=" << id_ << ", motor_id=" << motor_id
+                        << ", slave_position=" << slave.cfg.position()
+                        << ", object=" << hexIndex_(index)
+                        << ":" << static_cast<int>(subindex)
+                        << ", bit_len=" << static_cast<int>(bit_len)
+                        << ", value=" << raw_value
+                        << ", result=" << result
+                        << ", abort_code=" << hexIndex_(abort_code);
+        return false;
+    }
+    return true;
+}
+
+bool EthercatMotorBusRuntime::readSdoRaw_(const int motor_id,
+                                          const std::uint16_t index,
+                                          const std::uint8_t subindex,
+                                          const std::uint8_t bit_len,
+                                          std::uint64_t& value)
+{
+    if (!isSupportedBitLength_(bit_len)) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] unsupported SDO read bit length: "
+                        << static_cast<int>(bit_len)
+                        << ", object=" << hexIndex_(index)
+                        << ":" << static_cast<int>(subindex)
+                        << ", motor_id=" << motor_id;
+        return false;
+    }
+
+    auto slave_it = slaves_by_motor_id_.find(motor_id);
+    if (slave_it == slaves_by_motor_id_.end()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing motor for SDO read: "
+                        << motor_id << ", object=" << hexIndex_(index)
+                        << ":" << static_cast<int>(subindex);
+        return false;
+    }
+    const auto& slave = slave_it->second;
+    if (!master_) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] runtime is not initialized for SDO read: "
+                        << id_ << ", motor_id=" << motor_id
+                        << ", object=" << hexIndex_(index)
+                        << ":" << static_cast<int>(subindex);
+        return false;
+    }
+
+    std::array<std::uint8_t, 4> data{};
+    const auto data_size = static_cast<std::size_t>(bit_len / 8);
+    std::size_t result_size = 0;
+    std::uint32_t abort_code = 0;
+    const int result = ecrt_master_sdo_upload(
+        master_,
+        static_cast<std::uint16_t>(slave.cfg.position()),
+        index,
+        subindex,
+        data.data(),
+        data_size,
+        &result_size,
+        &abort_code);
+    if (result != 0 || result_size != data_size) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to read SDO: "
+                        << "group=" << id_ << ", motor_id=" << motor_id
+                        << ", slave_position=" << slave.cfg.position()
+                        << ", object=" << hexIndex_(index)
+                        << ":" << static_cast<int>(subindex)
+                        << ", bit_len=" << static_cast<int>(bit_len)
+                        << ", result=" << result
+                        << ", result_size=" << result_size
+                        << ", abort_code=" << hexIndex_(abort_code);
+        return false;
+    }
+
+    switch (bit_len) {
+        case 8:
+            value = EC_READ_U8(data.data());
+            break;
+        case 16:
+            value = EC_READ_U16(data.data());
+            break;
+        case 32:
+            value = EC_READ_U32(data.data());
+            break;
+        default:
+            return false;
+    }
+    value = maskValue_(value, bit_len);
+    return true;
+}
+
+std::uint32_t EthercatMotorBusRuntime::pdoEntryKey_(const std::uint16_t index,
+                                                    const std::uint8_t subindex)
+{
+    return (static_cast<std::uint32_t>(index) << 8U) | subindex;
+}
+
+std::string EthercatMotorBusRuntime::hexIndex_(const std::uint32_t index)
+{
+    std::ostringstream oss;
+    oss << "0x" << std::hex << std::uppercase << index;
+    return oss.str();
+}
+
+std::uint64_t EthercatMotorBusRuntime::maskValue_(const std::uint64_t value,
+                                                  const std::uint8_t bit_len)
+{
+    switch (bit_len) {
+        case 8:
+            return value & 0xFFU;
+        case 16:
+            return value & 0xFFFFU;
+        case 32:
+            return value & 0xFFFFFFFFULL;
+        default:
+            return value;
+    }
+}
+
+bool EthercatMotorBusRuntime::isSupportedBitLength_(const std::uint8_t bit_len)
+{
+    return bit_len == 8 || bit_len == 16 || bit_len == 32;
+}
+
+std::uint64_t EthercatMotorBusRuntime::readEntryValue_(const std::uint8_t* domain_data,
+                                                       const PdoEntryRuntime& entry)
+{
+    switch (entry.cfg.bit_len) {
+        case 8:
+            return EC_READ_U8(domain_data + entry.offset);
+        case 16:
+            return EC_READ_U16(domain_data + entry.offset);
+        case 32:
+            return EC_READ_U32(domain_data + entry.offset);
+        default:
+            return 0;
+    }
+}
+
+void EthercatMotorBusRuntime::writeEntryValue_(std::uint8_t* domain_data,
+                                               const PdoEntryRuntime& entry)
+{
+    switch (entry.cfg.bit_len) {
+        case 8:
+            EC_WRITE_U8(domain_data + entry.offset, static_cast<std::uint8_t>(entry.value));
+            break;
+        case 16:
+            EC_WRITE_U16(domain_data + entry.offset, static_cast<std::uint16_t>(entry.value));
+            break;
+        case 32:
+            EC_WRITE_U32(domain_data + entry.offset, static_cast<std::uint32_t>(entry.value));
+            break;
+        default:
+            break;
+    }
+}
+
+std::uint64_t EthercatMotorBusRuntime::steadyTimeNs_()
+{
+    return timePointNs_(std::chrono::steady_clock::now());
+}
+
+std::uint64_t EthercatMotorBusRuntime::timePointNs_(
+    const std::chrono::steady_clock::time_point time_point)
+{
+    const auto time_since_epoch = time_point.time_since_epoch();
+    return static_cast<std::uint64_t>(
+        std::chrono::duration_cast<std::chrono::nanoseconds>(time_since_epoch).count());
+}
+
+std::uint32_t EthercatMotorBusRuntime::usToNs_(const std::uint32_t value_us)
+{
+    return value_us * 1000U;
+}
+
+std::int32_t EthercatMotorBusRuntime::usToNs_(const std::int32_t value_us)
+{
+    return value_us * 1000;
 }
 
 } // namespace cmvr::device

cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp

@@ -0,0 +1,102 @@
+#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
+
+#include <algorithm>
+#include <chrono>
+#include <cstdint>
+#include <iostream>
+#include <string>
+#include <thread>
+
+#include <gtest/gtest.h>
+
+#include "cmvr/msgs/cia402.pb.h"
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h"
+
+namespace cmvr::device {
+namespace {
+
+
+
+config::MotorGroupConfig createSingleSlaveGroup()
+{
+    config::MotorGroupConfig group;
+    group.set_id("ethercat_real_test");
+    group.set_bus_type(config::MOTOR_BUS_ETHERCAT);
+    group.set_vendor(config::MOTOR_VENDOR_EYOU);
+    group.set_protocol(config::MOTOR_PROTOCOL_ETHERCAT_CIA402);
+
+    auto* ethercat = group.mutable_ethercat();
+    ethercat->set_master_index(0);
+    ethercat->set_cycle_us(1000);
+    ethercat->set_slave_op_timeout_ms(12000);
+    ethercat->set_slave_state_poll_period_ms(10);
+
+    auto* dc = ethercat->mutable_dc();
+    dc->set_enable(true);
+    dc->set_reference_motor_id(1);
+    dc->set_sync0_cycle_us(1000);
+    dc->set_sync0_shift_us(0);
+    dc->set_sync_reference_clock_period(1);
+    dc->set_assign_activate(768);
+    dc->set_sync_monitor_period_ms(1000);
+
+    auto* slave = ethercat->add_slaves();
+    slave->set_motor_id(1);
+    slave->set_alias(0);
+    slave->set_position(0);
+
+    return group;
+}
+
+} // namespace
+
+TEST(EthercatMotorBusRuntimeRealTest, InitStartAndReadStatusword)
+{
+
+
+    EthercatMotorBusRuntime runtime;
+    runtime.setPdoMapping(createEyouCia402PdoMapping());
+
+    ASSERT_TRUE(runtime.init(createSingleSlaveGroup()));
+    EXPECT_EQ(runtime.busType(), config::MOTOR_BUS_ETHERCAT);
+    EXPECT_TRUE(runtime.hasMotor(1));
+    EXPECT_NE(runtime.slaveForMotor(1), nullptr);
+    EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_CONTROL_WORD_6040, 0x00));
+    EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_STATUS_WORD_6041, 0x00));
+    EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_OPERATION_MODE_6060, 0x00));
+    EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_MODE_DISPLAY_6061, 0x00));
+
+    const bool started = runtime.start();
+    EXPECT_TRUE(started);
+    if (!started) {
+        runtime.stop();
+        return;
+    }
+
+    EXPECT_TRUE(runtime.writePdo<std::uint16_t>(1, msgs::CIA402_CONTROL_WORD_6040, 0x00, 0x0000));
+    EXPECT_TRUE(runtime.writePdo<std::int8_t>(1, msgs::CIA402_OPERATION_MODE_6060, 0x00, 0));
+
+    const int settle_ms = 1000;
+    std::this_thread::sleep_for(std::chrono::milliseconds(settle_ms));
+
+    std::uint16_t statusword = 0;
+    EXPECT_TRUE(runtime.readPdo<std::uint16_t>(1, msgs::CIA402_STATUS_WORD_6041, 0x00, statusword));
+
+    std::int8_t mode_display = 0;
+    EXPECT_TRUE(runtime.readPdo<std::int8_t>(1, msgs::CIA402_MODE_DISPLAY_6061, 0x00, mode_display));
+
+    std::cout << "CIA402 statusword: 0x" << std::hex << statusword
+              << ", mode display: " << std::dec << static_cast<int>(mode_display)
+              << std::endl;
+
+    const int hold_ms = 10000;
+    if (hold_ms > 0) {
+        std::cout << "Holding EtherCAT runtime for " << hold_ms
+                  << " ms. Check slave state in another terminal." << std::endl;
+        std::this_thread::sleep_for(std::chrono::milliseconds(hold_ms));
+    }
+
+    runtime.stop();
+}
+
+} // namespace cmvr::device

cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt

@@ -0,0 +1,49 @@
+add_library(ethercat_motor_driver SHARED
+        src/cia402/cia402_protocol.cpp
+        src/vendor/eyou/eyou_motor.cpp
+        src/vendor/eyou/eyou_motor_adapter.cpp
+)
+
+target_include_directories(ethercat_motor_driver
+        PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}/include
+)
+
+target_link_libraries(ethercat_motor_driver
+        PUBLIC
+        cmvr_es::device::motor_core
+        cmvr_es::device::motor_bus_runtime
+        PRIVATE
+        cmvr_es::proto
+        glog
+)
+
+add_library(cmvr_es::device::ethercat_motor_driver ALIAS ethercat_motor_driver)
+install(TARGETS ethercat_motor_driver LIBRARY DESTINATION lib)
+
+add_executable(eyou_motor_real_test
+        src/vendor/eyou/eyou_motor_real_test.cpp
+)
+
+target_link_libraries(eyou_motor_real_test
+        PRIVATE
+        cmvr_es::device::ethercat_motor_driver
+        gtest
+        gtest_main
+        pthread
+        glog
+)
+
+add_executable(eyou_motor_device_manager_real_test
+        src/vendor/eyou/eyou_motor_device_manager_real_test.cpp
+)
+
+target_link_libraries(eyou_motor_device_manager_real_test
+        PRIVATE
+        cmvr_es::device_manager
+        cmvr_es::device::motor_manager
+        gtest
+        gtest_main
+        pthread
+        glog
+)

cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h

@@ -0,0 +1,161 @@
+#ifndef CMVR_ES_CIA402_OBJECTS_H
+#define CMVR_ES_CIA402_OBJECTS_H
+
+#include <cstdint>
+
+namespace cmvr::device::cia402 {
+
+union Controlword {
+    std::uint16_t value;
+    struct {
+        std::uint16_t switch_on : 1;
+        std::uint16_t enable_voltage : 1;
+        std::uint16_t quick_stop : 1;
+        std::uint16_t enable_operation : 1;
+        std::uint16_t new_set_point : 1;
+        std::uint16_t change_set_immediately : 1;
+        std::uint16_t relative : 1;
+        std::uint16_t fault_reset : 1;
+        std::uint16_t halt : 1;
+        std::uint16_t reserved : 2;
+        std::uint16_t manufacturer_specific : 5;
+    };
+};
+
+union Statusword {
+    std::uint16_t value;
+    struct {
+        std::uint16_t ready_to_switch_on : 1;
+        std::uint16_t switched_on : 1;
+        std::uint16_t operation_enabled : 1;
+        std::uint16_t fault : 1;
+        std::uint16_t voltage_enabled : 1;
+        std::uint16_t quick_stop : 1;
+        std::uint16_t switch_on_disabled : 1;
+        std::uint16_t warning : 1;
+        std::uint16_t manufacturer_specific_8 : 1;
+        std::uint16_t remote : 1;
+        std::uint16_t target_reached : 1;
+        std::uint16_t internal_limit_active : 1;
+        std::uint16_t operation_mode_specific : 2;
+        std::uint16_t manufacturer_specific : 2;
+    };
+};
+
+static_assert(sizeof(Controlword) == sizeof(std::uint16_t));
+static_assert(sizeof(Statusword) == sizeof(std::uint16_t));
+
+enum class DeviceState {
+    SwitchOnDisabled,
+    ReadyToSwitchOn,
+    SwitchedOn,
+    OperationEnabled,
+};
+
+namespace detail {
+
+struct StateRule {
+    std::uint16_t relevant_bits;
+    std::uint16_t expected_bits;
+};
+
+inline StateRule stateRule(const DeviceState state)
+{
+    // CiA402 device states are matched by selected 0x6041 statusword bits.
+    switch (state) {
+        case DeviceState::SwitchOnDisabled:
+            return {0x004F, 0x0040};
+        case DeviceState::ReadyToSwitchOn:
+            return {0x006F, 0x0021};
+        case DeviceState::SwitchedOn:
+            return {0x006F, 0x0023};
+        case DeviceState::OperationEnabled:
+            return {0x006F, 0x0027};
+    }
+    return {0x006F, 0x0000};
+}
+
+} // namespace detail
+
+inline Controlword controlword(const std::uint16_t value)
+{
+    Controlword cw{};
+    cw.value = value;
+    return cw;
+}
+
+inline Statusword statusword(const std::uint16_t value)
+{
+    Statusword sw{};
+    sw.value = value;
+    return sw;
+}
+
+inline Controlword shutdownControlword()
+{
+    Controlword cw{};
+    cw.quick_stop = 1;
+    cw.enable_voltage = 1;
+    return cw;
+}
+
+inline Controlword switchOnControlword()
+{
+    auto cw = shutdownControlword();
+    cw.switch_on = 1;
+    return cw;
+}
+
+inline Controlword enableOperationControlword()
+{
+    auto cw = switchOnControlword();
+    cw.enable_operation = 1;
+    return cw;
+}
+
+inline Controlword quickStopControlword()
+{
+    auto cw = enableOperationControlword();
+    cw.quick_stop = 0;
+    return cw;
+}
+
+inline Controlword faultResetControlword()
+{
+    Controlword cw{};
+    cw.fault_reset = 1;
+    return cw;
+}
+
+inline Controlword profilePositionControlword(const bool new_set_point)
+{
+    auto cw = enableOperationControlword();
+    cw.change_set_immediately = 1;
+    cw.new_set_point = new_set_point ? 1 : 0;
+    return cw;
+}
+
+inline bool hasState(const Statusword status, const DeviceState state)
+{
+    const auto rule = detail::stateRule(state);
+    return (status.value & rule.relevant_bits) == rule.expected_bits;
+}
+
+inline bool isSwitchOnDisabled(const Statusword status)
+{
+    return hasState(status, DeviceState::SwitchOnDisabled);
+}
+
+inline bool isOperationEnabled(const Statusword status)
+{
+    return hasState(status, DeviceState::OperationEnabled);
+}
+
+inline bool targetReached(const Statusword status)
+{
+    return status.target_reached != 0;
+}
+
+} // namespace cmvr::device::cia402
+
+#endif // CMVR_ES_CIA402_OBJECTS_H

cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h

@@ -0,0 +1,116 @@
+#ifndef CMVR_ES_CIA402_PROTOCOL_H
+#define CMVR_ES_CIA402_PROTOCOL_H
+
+#include <cstdint>
+#include <memory>
+#include <unordered_map>
+
+#include "cmvr/config/motor_config/motor_config.pb.h"
+#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
+#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h"
+#include "devices/motor/motor_protocol_interface.h"
+
+namespace cmvr::device {
+
+class Cia402Protocol final : public MotorProtocolInterface {
+public:
+    explicit Cia402Protocol(std::shared_ptr<EthercatMotorBusRuntime> bus_runtime,
+                            const config::Cia402ProtocolConfig& config);
+    ~Cia402Protocol() override = default;
+
+    bool initNode(std::uint8_t node_id) override;
+
+    void setMode(std::uint8_t node_id, msgs::RunMode mode) override;
+    msgs::RunMode getMode(std::uint8_t node_id) override;
+    void setLimitQdd(std::uint8_t node_id, double u_qdd, double l_qdd) override;
+    void setLimitQd(std::uint8_t node_id, double qd) override;
+    void setLimitQ(std::uint8_t node_id, double ub, double lb) override;
+    bool calibrateZeroQ(std::uint8_t node_id) override;
+    bool reachedTargetQ(std::uint8_t node_id) override;
+    bool commandProfilePosition(std::uint8_t node_id,
+                                double target_q,
+                                double max_qd,
+                                double max_qdd) override;
+    bool commandProfileVelocity(std::uint8_t node_id,
+                                double target_qd,
+                                double max_qdd) override;
+    bool commandCyclicPosition(std::uint8_t node_id,
+                               double target_q,
+                               double target_qd) override;
+    bool commandCyclicVelocity(std::uint8_t node_id,
+                               double target_qd) override;
+    bool commandCyclicTorque(std::uint8_t node_id, double target_tau) override;
+    void setMotorConversion(std::uint8_t node_id,
+                            double encoder_counts_per_rev,
+                            double gear_ratio) override;
+    bool torqueOn(std::uint8_t node_id) override;
+    bool torqueOff(std::uint8_t node_id) override;
+    bool brakeRelease(std::uint8_t node_id) override;
+    bool quickStop(std::uint8_t node_id) override;
+
+    double getQ(std::uint8_t node_id) override;
+    double getQd(std::uint8_t node_id) override;
+    bool syncTargetToActualPosition(std::uint8_t node_id);
+
+private:
+    struct NodeState {
+        msgs::RunMode mode{msgs::RUN_MODE_CYCLIC_SYNC_POSITION};
+        cia402::Controlword controlword{};
+        std::int32_t target_position{0};
+        std::int32_t target_velocity{0};
+        std::int16_t target_torque{0};
+        std::int32_t profile_velocity{0};
+        std::int32_t profile_acceleration{0};
+        std::int32_t profile_deceleration{0};
+        double limit_q_lb{0.0};
+        double limit_q_ub{0.0};
+        double limit_qd{0.0};
+        double limit_qdd{0.0};
+        double encoder_counts_per_rev{0.0};
+        double gear_ratio{0.0};
+    };
+
+    static std::int8_t toCia402Mode_(msgs::RunMode mode);
+    static msgs::RunMode fromCia402Mode_(std::int8_t mode);
+    static cia402::Controlword nextControlword_(cia402::Statusword statusword);
+    static bool isOperationEnabled_(cia402::Statusword statusword);
+    static bool targetReached_(cia402::Statusword statusword);
+
+    std::int32_t radToCounts_(double angle_rad, const NodeState& state) const;
+    double countsToRad_(std::int32_t counts, const NodeState& state) const;
+    std::int32_t radPerSecToCounts_(double velocity_rad_s, const NodeState& state) const;
+    std::int32_t radPerSec2ToCounts_(double acceleration_rad_s2, const NodeState& state) const;
+    double countsToRadPerSec_(std::int32_t velocity_counts_s, const NodeState& state) const;
+    NodeState& nodeState_(std::uint8_t node_id);
+    const NodeState* findNodeState_(std::uint8_t node_id) const;
+    bool hasValidConversion_(std::uint8_t node_id, const NodeState& state) const;
+    bool validateNodePdos_(std::uint8_t node_id) const;
+    bool readStatusword_(std::uint8_t node_id, std::uint16_t& statusword) const;
+    bool readActualPosition_(std::uint8_t node_id, std::int32_t& actual_position) const;
+    bool readActualVelocity_(std::uint8_t node_id, std::int32_t& actual_velocity) const;
+    bool readModeDisplay_(std::uint8_t node_id, std::int8_t& mode_display) const;
+    bool writeControlword_(std::uint8_t node_id, cia402::Controlword controlword);
+    bool writeControlwordAndWait_(std::uint8_t node_id,
+                                  cia402::Controlword controlword,
+                                  cia402::DeviceState target_state,
+                                  const char* state_name);
+    bool waitStatus_(std::uint8_t node_id,
+                     cia402::DeviceState target_state,
+                     const char* state_name) const;
+    bool waitVelocityNearZero_(std::uint8_t node_id, const char* action_name) const;
+    bool writePositionLimitsToDictionary_(std::uint8_t node_id, const NodeState& state) const;
+    bool writeVelocityLimitToDictionary_(std::uint8_t node_id, const NodeState& state) const;
+    bool writeAccelerationLimitsToDictionary_(std::uint8_t node_id, const NodeState& state) const;
+    bool prepareSafeTargetsForMode_(std::uint8_t node_id, msgs::RunMode mode, NodeState& state);
+    void writeTargetsForMode_(std::uint8_t node_id, msgs::RunMode mode, const NodeState& state) const;
+    void writeProfilePositionTarget_(std::uint8_t node_id, NodeState& state);
+    void writeNode_(std::uint8_t node_id, NodeState& state);
+
+    std::shared_ptr<EthercatMotorBusRuntime> bus_runtime_;
+    config::Cia402ProtocolConfig config_;
+    std::unordered_map<std::uint8_t, NodeState> nodes_;
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_CIA402_PROTOCOL_H

cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h

@@ -0,0 +1,81 @@
+#ifndef CMVR_ES_EYOU_CIA402_PDO_MAPPING_H
+#define CMVR_ES_EYOU_CIA402_PDO_MAPPING_H
+
+#include <cstdint>
+#include <string>
+#include <utility>
+
+#include "cmvr/msgs/canopen.pb.h"
+#include "cmvr/msgs/cia402.pb.h"
+#include "devices/motor/bus_runtime/ethercat/include/ethercat_pdo_mapping.h"
+
+namespace cmvr::device {
+
+namespace eyou_cia402_pdo_mapping_detail {
+
+inline constexpr std::uint32_t VENDOR_ID = 0x00001097;
+inline constexpr std::uint32_t PRODUCT_CODE = 0x00002406;
+
+inline EthercatPdoEntryConfig entry(const std::uint16_t index,
+                                    const std::uint8_t subindex,
+                                    const std::uint8_t bit_len,
+                                    std::string name)
+{
+    EthercatPdoEntryConfig cfg;
+    cfg.index = index;
+    cfg.subindex = subindex;
+    cfg.bit_len = bit_len;
+    cfg.name = std::move(name);
+    cfg.padding = index == 0 || bit_len == 0;
+    return cfg;
+}
+
+} // namespace eyou_cia402_pdo_mapping_detail
+
+inline EthercatPdoMapping createEyouCia402PdoMapping()
+{
+    using namespace eyou_cia402_pdo_mapping_detail;
+
+    EthercatPdoConfig rx_pdo;
+    rx_pdo.index = msgs::CANOPEN_RPDO2_MAP_1601;
+    rx_pdo.sync_manager = 2;
+    rx_pdo.rx = true;
+    rx_pdo.entries = {
+        entry(msgs::CIA402_CONTROL_WORD_6040, 0x00, 16, "Control Word"),
+        entry(msgs::CIA402_TARGET_POSITION_607A, 0x00, 32, "Target Position"),
+        entry(msgs::CIA402_TARGET_VELOCITY_60FF, 0x00, 32, "Target Velocity"),
+        entry(msgs::CIA402_TARGET_TORQUE_6071, 0x00, 16, "Target Torque"),
+        entry(msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00, 32, "Profile Acceleration"),
+        entry(msgs::CIA402_PROFILE_DECELERATION_6084, 0x00, 32, "Profile Deceleration"),
+        entry(msgs::CIA402_PROFILE_VELOCITY_6081, 0x00, 32, "Profile Velocity"),
+        entry(msgs::CIA402_TORQUE_SLOPE_6087, 0x00, 32, "Torque Slope"),
+        entry(msgs::CIA402_OPERATION_MODE_6060, 0x00, 8, "Mode Of Operation"),
+        entry(0x0000, 0x00, 8, "Padding"),
+    };
+
+    EthercatPdoConfig tx_pdo;
+    tx_pdo.index = msgs::CANOPEN_TPDO1_MAP_1A00;
+    tx_pdo.sync_manager = 3;
+    tx_pdo.rx = false;
+    tx_pdo.entries = {
+        entry(msgs::CIA402_STATUS_WORD_6041, 0x00, 16, "Status Word"),
+        entry(msgs::CIA402_ACTUAL_POSITION_6064, 0x00, 32, "Actual Position"),
+        entry(msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00, 32, "Actual Velocity"),
+        entry(msgs::CIA402_ACTUAL_TORQUE_6077, 0x00, 16, "Actual Torque"),
+        entry(msgs::CIA402_MODE_DISPLAY_6061, 0x00, 8, "Mode Of Operation Display"),
+        entry(msgs::CIA402_ERROR_CODE_603F, 0x00, 16, "Error Code"),
+        entry(0x0000, 0x00, 8, "Padding"),
+    };
+
+    EthercatPdoMapping mapping;
+    mapping.vendor_id = VENDOR_ID;
+    mapping.product_code = PRODUCT_CODE;
+    mapping.name = "EYOU ServoModule ECAT V145 CiA402";
+    mapping.rx_pdos.push_back(std::move(rx_pdo));
+    mapping.tx_pdos.push_back(std::move(tx_pdo));
+    return mapping;
+}
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_EYOU_CIA402_PDO_MAPPING_H

cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h

@@ -0,0 +1,43 @@
+#ifndef CMVR_ES_EYOU_MOTOR_H
+#define CMVR_ES_EYOU_MOTOR_H
+
+#include <cstdint>
+#include <memory>
+
+#include "cmvr/config/motor_config/motor_config.pb.h"
+#include "devices/motor/abstract_motor.h"
+#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h"
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h"
+
+namespace cmvr::device {
+
+class EyouMotor final : public AbstractMotor {
+public:
+    EyouMotor(const config::MotorConfigItem& config,
+              std::shared_ptr<Cia402Protocol> cia402_protocol,
+              std::unique_ptr<EyouMotorAdapter> vendor_adapter);
+
+    std::string typeName() const override { return "EyouMotor"; }
+    bool init() override;
+    void setLimitQ(double ub, double lb) override;
+    void setLimitQd(double qd) override;
+    bool calibrateZeroQ() override;
+    bool brakeRelease() override;
+
+private:
+    bool hasDependencies_() const;
+    bool hasValidConversion_() const;
+    bool writeVendorPositionLimits_() const;
+    bool writeVendorVelocityLimit_() const;
+    std::int32_t radToCounts_(double angle_rad) const;
+    std::uint32_t radPerSecToCounts_(double velocity_rad_s) const;
+
+    std::shared_ptr<Cia402Protocol> cia402_protocol_;
+    std::unique_ptr<EyouMotorAdapter> vendor_adapter_;
+    double encoder_counts_per_rev_{0.0};
+    double gear_ratio_{0.0};
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_EYOU_MOTOR_H

cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h

@@ -0,0 +1,33 @@
+#ifndef CMVR_ES_EYOU_MOTOR_ADAPTER_H
+#define CMVR_ES_EYOU_MOTOR_ADAPTER_H
+
+#include <cstdint>
+#include <memory>
+
+#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
+#include "devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h"
+
+namespace cmvr::device {
+
+class EyouMotorAdapter final : public MotorVendorAdapter {
+public:
+    explicit EyouMotorAdapter(std::shared_ptr<EthercatMotorBusRuntime> bus_runtime);
+    ~EyouMotorAdapter() override = default;
+
+    bool initNode(std::uint8_t node_id) override;
+    bool writePositionLimits(std::uint8_t node_id,
+                             std::int32_t lower_limit,
+                             std::int32_t upper_limit) override;
+    bool writeVelocityLimit(std::uint8_t node_id,
+                            std::uint32_t velocity_limit) override;
+    bool calibrateZero(std::uint8_t node_id,
+                       std::int32_t& zeroed_position) override;
+    bool brakeRelease(std::uint8_t node_id) override;
+
+private:
+    std::shared_ptr<EthercatMotorBusRuntime> bus_runtime_;
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_EYOU_MOTOR_ADAPTER_H

cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h

@@ -0,0 +1,20 @@
+#ifndef CMVR_ES_EYOU_OBJECTS_H
+#define CMVR_ES_EYOU_OBJECTS_H
+
+#include <cstdint>
+
+namespace cmvr::device::eyou {
+
+inline constexpr std::uint16_t EYOU_SOFT_LIMIT_STATE_2003 = 0x2003;
+
+inline constexpr std::uint16_t EYOU_BRAKE_CONTROL_2014 = 0x2014;
+
+inline constexpr std::uint16_t EYOU_OVER_SPEED_THRESHOLD_2024 = 0x2024;
+inline constexpr std::uint16_t EYOU_FIRST_ENCODER_VALUE_202A = 0x202A;
+inline constexpr std::uint16_t EYOU_SECOND_ENCODER_VALUE_202B = 0x202B;
+
+inline constexpr std::uint16_t EYOU_STORE_PARAMETERS_1010 = 0x1010;
+
+} // namespace cmvr::device::eyou
+
+#endif // CMVR_ES_EYOU_OBJECTS_H

cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h

@@ -0,0 +1,25 @@
+#ifndef CMVR_ES_MOTOR_VENDOR_ADAPTER_H
+#define CMVR_ES_MOTOR_VENDOR_ADAPTER_H
+
+#include <cstdint>
+
+namespace cmvr::device {
+
+class MotorVendorAdapter {
+public:
+    virtual ~MotorVendorAdapter() = default;
+
+    virtual bool initNode(std::uint8_t node_id) = 0;
+    virtual bool writePositionLimits(std::uint8_t node_id,
+                                     std::int32_t lower_limit,
+                                     std::int32_t upper_limit) = 0;
+    virtual bool writeVelocityLimit(std::uint8_t node_id,
+                                    std::uint32_t velocity_limit) = 0;
+    virtual bool calibrateZero(std::uint8_t node_id,
+                               std::int32_t& zeroed_position) = 0;
+    virtual bool brakeRelease(std::uint8_t node_id) = 0;
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_MOTOR_VENDOR_ADAPTER_H

cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp

@@ -0,0 +1,872 @@
+#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h"
+
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <thread>
+#include <utility>
+
+#include "common/base/logging/logger.h"
+#include "cmvr/msgs/cia402.pb.h"
+#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h"
+
+namespace cmvr::device {
+
+Cia402Protocol::Cia402Protocol(std::shared_ptr<EthercatMotorBusRuntime> bus_runtime,
+                               const config::Cia402ProtocolConfig& config)
+    : bus_runtime_(std::move(bus_runtime)),
+      config_(config)
+{
+    comm_proto = CommProto::ETHERCAT;
+}
+
+bool Cia402Protocol::initNode(const std::uint8_t node_id)
+{
+    if (!bus_runtime_ || !bus_runtime_->hasMotor(node_id)) {
+        CMVR_LOG(ERROR) << "[Cia402Protocol] missing EtherCAT motor: "
+                        << static_cast<int>(node_id);
+        return false;
+    }
+
+    if (!validateNodePdos_(node_id)) {
+        return false;
+    }
+    auto& state = nodeState_(node_id);
+    std::int32_t actual_position = 0;
+    if (readActualPosition_(node_id, actual_position)) {
+        state.target_position = actual_position;
+    }
+    writeNode_(node_id, state);
+    return true;
+}
+
+bool Cia402Protocol::commandProfilePosition(const std::uint8_t node_id,
+                                                    const double target_q,
+                                                    const double max_qd,
+                                                    const double max_qdd)
+{
+    auto& state = nodeState_(node_id);
+    if (!hasValidConversion_(node_id, state)) {
+        return false;
+    }
+    state.target_position = radToCounts_(target_q, state);
+    state.profile_velocity = std::abs(radPerSecToCounts_(max_qd, state));
+    if (max_qdd > 0.0) {
+        const auto profile_acceleration = std::abs(radPerSec2ToCounts_(max_qdd, state));
+        state.profile_acceleration = profile_acceleration;
+        state.profile_deceleration = profile_acceleration;
+    }
+    writeProfilePositionTarget_(node_id, state);
+    return true;
+}
+
+bool Cia402Protocol::commandProfileVelocity(const std::uint8_t node_id,
+                                                    const double target_qd,
+                                                    const double max_qdd)
+{
+    auto& state = nodeState_(node_id);
+    if (!hasValidConversion_(node_id, state)) {
+        return false;
+    }
+    state.target_velocity = radPerSecToCounts_(target_qd, state);
+    if (max_qdd > 0.0) {
+        const auto profile_acceleration = std::abs(radPerSec2ToCounts_(max_qdd, state));
+        state.profile_acceleration = profile_acceleration;
+        state.profile_deceleration = profile_acceleration;
+    }
+    writeNode_(node_id, state);
+    return true;
+}
+
+bool Cia402Protocol::commandCyclicPosition(const std::uint8_t node_id,
+                                                   const double target_q,
+                                                   const double target_qd)
+{
+    auto& state = nodeState_(node_id);
+    if (!hasValidConversion_(node_id, state)) {
+        return false;
+    }
+    state.target_position = radToCounts_(target_q, state);
+    state.target_velocity = radPerSecToCounts_(target_qd, state);
+    writeNode_(node_id, state);
+    return true;
+}
+
+bool Cia402Protocol::commandCyclicVelocity(const std::uint8_t node_id,
+                                                   const double target_qd)
+{
+    auto& state = nodeState_(node_id);
+    if (!hasValidConversion_(node_id, state)) {
+        return false;
+    }
+    state.target_velocity = radPerSecToCounts_(target_qd, state);
+    writeNode_(node_id, state);
+    return true;
+}
+
+bool Cia402Protocol::commandCyclicTorque(const std::uint8_t node_id,
+                                                 const double target_tau)
+{
+    (void)target_tau;
+    CMVR_LOG(ERROR) << "[Cia402Protocol] cyclic torque command is not implemented, node="
+                    << static_cast<int>(node_id);
+    return false;
+}
+
+void Cia402Protocol::setMode(const std::uint8_t node_id, const msgs::RunMode mode)
+{
+    auto& state = nodeState_(node_id);
+    if (!prepareSafeTargetsForMode_(node_id, mode, state)) {
+        return;
+    }
+    writeTargetsForMode_(node_id, mode, state);
+    state.mode = mode;
+    writeNode_(node_id, state);
+}
+
+msgs::RunMode Cia402Protocol::getMode(const std::uint8_t node_id)
+{
+    std::int8_t mode_display = 0;
+    if (readModeDisplay_(node_id, mode_display)) {
+        return fromCia402Mode_(mode_display);
+    }
+    return nodeState_(node_id).mode;
+}
+
+void Cia402Protocol::setLimitQdd(const std::uint8_t node_id,
+                                         const double u_qdd,
+                                         const double l_qdd)
+{
+    auto& state = nodeState_(node_id);
+    state.limit_qdd = std::max(std::abs(u_qdd), std::abs(l_qdd));
+    if (hasValidConversion_(node_id, state)) {
+        const auto profile_acceleration = std::abs(radPerSec2ToCounts_(state.limit_qdd, state));
+        state.profile_acceleration = profile_acceleration;
+        state.profile_deceleration = profile_acceleration;
+        writeAccelerationLimitsToDictionary_(node_id, state);
+    }
+}
+
+void Cia402Protocol::setLimitQd(const std::uint8_t node_id, const double qd)
+{
+    auto& state = nodeState_(node_id);
+    state.limit_qd = std::abs(qd);
+    if (hasValidConversion_(node_id, state)) {
+        state.profile_velocity = std::abs(radPerSecToCounts_(state.limit_qd, state));
+        writeVelocityLimitToDictionary_(node_id, state);
+    }
+}
+
+void Cia402Protocol::setLimitQ(const std::uint8_t node_id,
+                                       const double ub,
+                                       const double lb)
+{
+    auto& state = nodeState_(node_id);
+    state.limit_q_ub = ub;
+    state.limit_q_lb = lb;
+    if (hasValidConversion_(node_id, state)) {
+        writePositionLimitsToDictionary_(node_id, state);
+    }
+}
+
+bool Cia402Protocol::calibrateZeroQ(const std::uint8_t node_id)
+{
+    CMVR_LOG(ERROR) << "[Cia402Protocol] zero calibration is vendor-specific, node="
+                    << static_cast<int>(node_id);
+    return false;
+}
+
+bool Cia402Protocol::reachedTargetQ(const std::uint8_t node_id)
+{
+    std::uint16_t statusword = 0;
+    if (!readStatusword_(node_id, statusword)) {
+        return false;
+    }
+    return targetReached_(cia402::statusword(statusword));
+}
+
+void Cia402Protocol::setMotorConversion(
+    const std::uint8_t node_id,
+    const double encoder_counts_per_rev,
+    const double gear_ratio)
+{
+    auto& state = nodeState_(node_id);
+    state.encoder_counts_per_rev = encoder_counts_per_rev;
+    state.gear_ratio = gear_ratio;
+}
+
+bool Cia402Protocol::torqueOn(const std::uint8_t node_id)
+{
+    if (!bus_runtime_) {
+        return false;
+    }
+
+    auto& state = nodeState_(node_id);
+    std::uint16_t statusword = 0;
+    if (readStatusword_(node_id, statusword) && cia402::statusword(statusword).fault != 0) {
+        if (!writeControlword_(node_id, cia402::faultResetControlword())) {
+            return false;
+        }
+        std::this_thread::sleep_for(
+            std::chrono::milliseconds(config_.status_poll_period_ms()));
+    }
+
+    if (!prepareSafeTargetsForMode_(node_id, msgs::RUN_MODE_PROFILE_POSITION, state)) {
+        return false;
+    }
+    state.mode = msgs::RUN_MODE_PROFILE_POSITION;
+    state.target_velocity = 0;
+    state.target_torque = 0;
+    writeTargetsForMode_(node_id, state.mode, state);
+    if (!bus_runtime_->writePdo<std::int8_t>(node_id, msgs::CIA402_OPERATION_MODE_6060, 0x00,
+                                  toCia402Mode_(state.mode))) {
+        return false;
+    }
+
+    if (!writeControlwordAndWait_(node_id, cia402::shutdownControlword(),
+                                  cia402::DeviceState::ReadyToSwitchOn,
+                                  "Ready To Switch On")) {
+        return false;
+    }
+    if (!writeControlwordAndWait_(node_id, cia402::switchOnControlword(),
+                                  cia402::DeviceState::SwitchedOn,
+                                  "Switched On")) {
+        return false;
+    }
+    if (!writeControlwordAndWait_(node_id, cia402::enableOperationControlword(),
+                                  cia402::DeviceState::OperationEnabled,
+                                  "Operation Enabled")) {
+        return false;
+    }
+
+    std::int32_t actual_position = 0;
+    if (readActualPosition_(node_id, actual_position)) {
+        state.target_position = actual_position;
+    }
+    writeTargetsForMode_(node_id, state.mode, state);
+
+    state.controlword = cia402::profilePositionControlword(true);
+    if (!writeControlword_(node_id, state.controlword)) {
+        return false;
+    }
+    std::this_thread::sleep_for(
+        std::chrono::milliseconds(config_.profile_position_trigger_delay_ms()));
+
+    state.controlword = cia402::profilePositionControlword(false);
+    return writeControlword_(node_id, state.controlword);
+}
+
+bool Cia402Protocol::torqueOff(const std::uint8_t node_id)
+{
+    if (!bus_runtime_) {
+        return false;
+    }
+
+    auto& state = nodeState_(node_id);
+    std::int32_t actual_position = 0;
+    if (readActualPosition_(node_id, actual_position)) {
+        state.target_position = actual_position;
+    }
+    state.target_velocity = 0;
+    state.target_torque = 0;
+    writeTargetsForMode_(node_id, state.mode, state);
+    waitVelocityNearZero_(node_id, "torqueOff");
+
+    if (!writeControlwordAndWait_(node_id, cia402::switchOnControlword(),
+                                  cia402::DeviceState::SwitchedOn,
+                                  "Switched On")) {
+        return false;
+    }
+    if (!writeControlwordAndWait_(node_id, cia402::shutdownControlword(),
+                                  cia402::DeviceState::ReadyToSwitchOn,
+                                  "Ready To Switch On")) {
+        return false;
+    }
+
+    return true;
+}
+
+bool Cia402Protocol::brakeRelease(const std::uint8_t node_id)
+{
+    CMVR_LOG(ERROR) << "[Cia402Protocol] brake release is vendor-specific, node="
+                    << static_cast<int>(node_id);
+    return false;
+}
+
+bool Cia402Protocol::quickStop(const std::uint8_t node_id)
+{
+    if (!bus_runtime_) {
+        return false;
+    }
+
+    auto& state = nodeState_(node_id);
+    state.target_velocity = 0;
+    state.target_torque = 0;
+    writeTargetsForMode_(node_id, state.mode, state);
+
+    state.controlword = cia402::quickStopControlword();
+    if (!writeControlword_(node_id, state.controlword)) {
+        return false;
+    }
+    return waitVelocityNearZero_(node_id, "quickStop");
+}
+
+double Cia402Protocol::getQ(const std::uint8_t node_id)
+{
+    std::int32_t actual_position = 0;
+    if (!readActualPosition_(node_id, actual_position)) {
+        return 0.0;
+    }
+    const auto& state = nodeState_(node_id);
+    if (!hasValidConversion_(node_id, state)) {
+        return 0.0;
+    }
+    return countsToRad_(actual_position, state);
+}
+
+double Cia402Protocol::getQd(const std::uint8_t node_id)
+{
+    std::int32_t actual_velocity = 0;
+    if (!readActualVelocity_(node_id, actual_velocity)) {
+        return 0.0;
+    }
+    const auto& state = nodeState_(node_id);
+    if (!hasValidConversion_(node_id, state)) {
+        return 0.0;
+    }
+    return countsToRadPerSec_(actual_velocity, state);
+}
+
+bool Cia402Protocol::syncTargetToActualPosition(const std::uint8_t node_id)
+{
+    std::int32_t actual_position = 0;
+    if (!readActualPosition_(node_id, actual_position)) {
+        CMVR_LOG(ERROR) << "[Cia402Protocol] failed to read actual position, node="
+                        << static_cast<int>(node_id);
+        return false;
+    }
+
+    auto& state = nodeState_(node_id);
+    state.target_position = actual_position;
+    state.target_velocity = 0;
+    state.target_torque = 0;
+    writeTargetsForMode_(node_id, state.mode, state);
+    return true;
+}
+
+std::int8_t Cia402Protocol::toCia402Mode_(const msgs::RunMode mode)
+{
+    switch (mode) {
+        case msgs::RUN_MODE_PROFILE_POSITION:
+            return 1;
+        case msgs::RUN_MODE_PROFILE_VELOCITY:
+            return 3;
+        case msgs::RUN_MODE_HOMING:
+            return 6;
+        case msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
+            return 8;
+        case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY:
+            return 9;
+        case msgs::RUN_MODE_CYCLIC_SYNC_CURRENT:
+            return 10;
+        default:
+            return 8;
+    }
+}
+
+msgs::RunMode Cia402Protocol::fromCia402Mode_(const std::int8_t mode)
+{
+    switch (mode) {
+        case 1:
+            return msgs::RUN_MODE_PROFILE_POSITION;
+        case 3:
+            return msgs::RUN_MODE_PROFILE_VELOCITY;
+        case 6:
+            return msgs::RUN_MODE_HOMING;
+        case 8:
+            return msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
+        case 9:
+            return msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY;
+        case 10:
+            return msgs::RUN_MODE_CYCLIC_SYNC_CURRENT;
+        default:
+            return msgs::RUN_MODE_UNSPECIFIED;
+    }
+}
+
+cia402::Controlword Cia402Protocol::nextControlword_(const cia402::Statusword statusword)
+{
+    if (statusword.fault != 0) {
+        return cia402::faultResetControlword();
+    }
+    if (cia402::hasState(statusword, cia402::DeviceState::SwitchOnDisabled)) {
+        return cia402::shutdownControlword();
+    }
+    if (cia402::hasState(statusword, cia402::DeviceState::ReadyToSwitchOn)) {
+        return cia402::switchOnControlword();
+    }
+    if (cia402::hasState(statusword, cia402::DeviceState::SwitchedOn)) {
+        return cia402::enableOperationControlword();
+    }
+    if (cia402::hasState(statusword, cia402::DeviceState::OperationEnabled)) {
+        return cia402::enableOperationControlword();
+    }
+    return cia402::shutdownControlword();
+}
+
+bool Cia402Protocol::isOperationEnabled_(const cia402::Statusword statusword)
+{
+    return cia402::isOperationEnabled(statusword);
+}
+
+bool Cia402Protocol::targetReached_(const cia402::Statusword statusword)
+{
+    return cia402::targetReached(statusword);
+}
+
+std::int32_t Cia402Protocol::radToCounts_(const double angle_rad,
+                                                  const NodeState& state) const
+{
+    const double rev = angle_rad / (2.0 * M_PI);
+    return static_cast<std::int32_t>(
+        std::llround(rev * state.gear_ratio * state.encoder_counts_per_rev));
+}
+
+double Cia402Protocol::countsToRad_(const std::int32_t counts,
+                                            const NodeState& state) const
+{
+    return static_cast<double>(counts) /
+           (state.gear_ratio * state.encoder_counts_per_rev) * 2.0 * M_PI;
+}
+
+std::int32_t Cia402Protocol::radPerSecToCounts_(
+    const double velocity_rad_s,
+    const NodeState& state) const
+{
+    const double rev_per_sec = velocity_rad_s / (2.0 * M_PI);
+    return static_cast<std::int32_t>(
+        std::llround(rev_per_sec * state.gear_ratio * state.encoder_counts_per_rev));
+}
+
+std::int32_t Cia402Protocol::radPerSec2ToCounts_(
+    const double acceleration_rad_s2,
+    const NodeState& state) const
+{
+    const double rev_per_sec2 = acceleration_rad_s2 / (2.0 * M_PI);
+    return static_cast<std::int32_t>(
+        std::llround(rev_per_sec2 * state.gear_ratio * state.encoder_counts_per_rev));
+}
+
+double Cia402Protocol::countsToRadPerSec_(
+    const std::int32_t velocity_counts_s,
+    const NodeState& state) const
+{
+    return static_cast<double>(velocity_counts_s) /
+           (state.gear_ratio * state.encoder_counts_per_rev) * 2.0 * M_PI;
+}
+
+Cia402Protocol::NodeState& Cia402Protocol::nodeState_(const std::uint8_t node_id)
+{
+    return nodes_[node_id];
+}
+
+const Cia402Protocol::NodeState* Cia402Protocol::findNodeState_(
+    const std::uint8_t node_id) const
+{
+    const auto it = nodes_.find(node_id);
+    if (it == nodes_.end()) {
+        return nullptr;
+    }
+    return &it->second;
+}
+
+bool Cia402Protocol::hasValidConversion_(const std::uint8_t node_id,
+                                                 const NodeState& state) const
+{
+    if (state.encoder_counts_per_rev > 0.0 && state.gear_ratio > 0.0) {
+        return true;
+    }
+    CMVR_LOG(ERROR) << "[Cia402Protocol] missing conversion config for node "
+                    << static_cast<int>(node_id)
+                    << ": encoder_counts_per_rev=" << state.encoder_counts_per_rev
+                    << ", gear_ratio=" << state.gear_ratio;
+    return false;
+}
+
+bool Cia402Protocol::validateNodePdos_(const std::uint8_t node_id) const
+{
+    if (!bus_runtime_) {
+        return false;
+    }
+    struct RequiredEntry {
+        std::uint16_t index;
+        std::uint8_t subindex;
+        const char* name;
+    };
+    const RequiredEntry required[] = {
+        {msgs::CIA402_CONTROL_WORD_6040, 0x00, "controlword"},
+        {msgs::CIA402_TARGET_POSITION_607A, 0x00, "target position"},
+        {msgs::CIA402_TARGET_VELOCITY_60FF, 0x00, "target velocity"},
+        {msgs::CIA402_TARGET_TORQUE_6071, 0x00, "target torque"},
+        {msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00, "profile acceleration"},
+        {msgs::CIA402_PROFILE_DECELERATION_6084, 0x00, "profile deceleration"},
+        {msgs::CIA402_PROFILE_VELOCITY_6081, 0x00, "profile velocity"},
+        {msgs::CIA402_OPERATION_MODE_6060, 0x00, "operation mode"},
+        {msgs::CIA402_STATUS_WORD_6041, 0x00, "statusword"},
+        {msgs::CIA402_ACTUAL_POSITION_6064, 0x00, "actual position"},
+        {msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00, "actual velocity"},
+        {msgs::CIA402_ACTUAL_TORQUE_6077, 0x00, "actual torque"},
+        {msgs::CIA402_MODE_DISPLAY_6061, 0x00, "mode display"},
+        {msgs::CIA402_ERROR_CODE_603F, 0x00, "error code"},
+    };
+
+    for (const auto& entry : required) {
+        if (!bus_runtime_->hasPdoEntry(node_id, entry.index, entry.subindex)) {
+            CMVR_LOG(ERROR) << "[Cia402Protocol] missing PDO entry for node "
+                            << static_cast<int>(node_id)
+                            << ": " << entry.name
+                            << " 0x" << std::hex << entry.index
+                            << ":" << static_cast<int>(entry.subindex) << std::dec;
+            return false;
+        }
+    }
+    return true;
+}
+
+bool Cia402Protocol::readStatusword_(const std::uint8_t node_id,
+                                             std::uint16_t& statusword) const
+{
+    return bus_runtime_ &&
+           bus_runtime_->readPdo<std::uint16_t>(node_id, msgs::CIA402_STATUS_WORD_6041, 0x00,
+                                    statusword);
+}
+
+bool Cia402Protocol::readActualPosition_(const std::uint8_t node_id,
+                                                 std::int32_t& actual_position) const
+{
+    return bus_runtime_ &&
+           bus_runtime_->readPdo<std::int32_t>(node_id, msgs::CIA402_ACTUAL_POSITION_6064, 0x00,
+                                    actual_position);
+}
+
+bool Cia402Protocol::readActualVelocity_(const std::uint8_t node_id,
+                                                 std::int32_t& actual_velocity) const
+{
+    return bus_runtime_ &&
+           bus_runtime_->readPdo<std::int32_t>(node_id, msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00,
+                                    actual_velocity);
+}
+
+bool Cia402Protocol::readModeDisplay_(const std::uint8_t node_id,
+                                              std::int8_t& mode_display) const
+{
+    return bus_runtime_ &&
+           bus_runtime_->readPdo<std::int8_t>(node_id, msgs::CIA402_MODE_DISPLAY_6061, 0x00,
+                                   mode_display);
+}
+
+bool Cia402Protocol::writeControlword_(const std::uint8_t node_id,
+                                       const cia402::Controlword controlword)
+{
+    if (!bus_runtime_) {
+        return false;
+    }
+    auto& state = nodeState_(node_id);
+    state.controlword = controlword;
+    return bus_runtime_->writePdo<std::uint16_t>(node_id, msgs::CIA402_CONTROL_WORD_6040,
+                                                 0x00, controlword.value);
+}
+
+bool Cia402Protocol::writeControlwordAndWait_(
+    const std::uint8_t node_id,
+    const cia402::Controlword controlword,
+    const cia402::DeviceState target_state,
+    const char* state_name)
+{
+    if (!writeControlword_(node_id, controlword)) {
+        return false;
+    }
+    return waitStatus_(node_id, target_state, state_name);
+}
+
+bool Cia402Protocol::waitStatus_(const std::uint8_t node_id,
+                                 const cia402::DeviceState target_state,
+                                 const char* state_name) const
+{
+    const auto deadline = std::chrono::steady_clock::now() +
+                          std::chrono::milliseconds(config_.state_transition_timeout_ms());
+    std::uint16_t last_statusword = 0;
+    do {
+        if (readStatusword_(node_id, last_statusword) &&
+            cia402::hasState(cia402::statusword(last_statusword), target_state)) {
+            return true;
+        }
+        std::this_thread::sleep_for(
+            std::chrono::milliseconds(config_.status_poll_period_ms()));
+    } while (std::chrono::steady_clock::now() < deadline);
+
+    CMVR_LOG(ERROR) << "[Cia402Protocol] timeout waiting for "
+                    << state_name << ", node=" << static_cast<int>(node_id)
+                    << ", last_statusword=0x" << std::hex << last_statusword << std::dec;
+    return false;
+}
+
+bool Cia402Protocol::waitVelocityNearZero_(const std::uint8_t node_id,
+                                                   const char* action_name) const
+{
+    const auto* state = findNodeState_(node_id);
+    if (state == nullptr || !hasValidConversion_(node_id, *state)) {
+        return false;
+    }
+
+    const auto tolerance_counts = std::max<std::int32_t>(
+        1, std::abs(radPerSecToCounts_(config_.stopped_velocity_tolerance_rad_s(), *state)));
+    const auto deadline = std::chrono::steady_clock::now() +
+                          std::chrono::milliseconds(config_.velocity_stop_timeout_ms());
+    std::int32_t last_velocity = 0;
+    do {
+        if (readActualVelocity_(node_id, last_velocity) &&
+            std::abs(last_velocity) <= tolerance_counts) {
+            return true;
+        }
+        std::this_thread::sleep_for(
+            std::chrono::milliseconds(config_.status_poll_period_ms()));
+    } while (std::chrono::steady_clock::now() < deadline);
+
+    CMVR_LOG(ERROR) << "[Cia402Protocol] timeout waiting velocity near zero "
+                    << "during " << action_name
+                    << ", node=" << static_cast<int>(node_id)
+                    << ", last_velocity=" << last_velocity
+                    << ", tolerance=" << tolerance_counts;
+    return false;
+}
+
+bool Cia402Protocol::writePositionLimitsToDictionary_(
+    const std::uint8_t node_id,
+    const NodeState& state) const
+{
+    if (!bus_runtime_) {
+        return false;
+    }
+    if (!std::isfinite(state.limit_q_lb) || !std::isfinite(state.limit_q_ub) ||
+        state.limit_q_ub <= state.limit_q_lb) {
+        return true;
+    }
+
+    const auto lower_limit = radToCounts_(state.limit_q_lb, state);
+    const auto upper_limit = radToCounts_(state.limit_q_ub, state);
+
+    const bool ok =
+        bus_runtime_->writeSdo<std::int32_t>(node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D,
+                                             0x01, lower_limit) &&
+        bus_runtime_->writeSdo<std::int32_t>(node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D,
+                                             0x02, upper_limit);
+    if (!ok) {
+        CMVR_LOG(ERROR) << "[Cia402Protocol] failed to write software position "
+                        << "limits to dictionary, node=" << static_cast<int>(node_id)
+                        << ", lower=" << lower_limit
+                        << ", upper=" << upper_limit;
+    }
+    return ok;
+}
+
+bool Cia402Protocol::writeVelocityLimitToDictionary_(
+    const std::uint8_t node_id,
+    const NodeState& state) const
+{
+    if (!bus_runtime_) {
+        return false;
+    }
+    if (!std::isfinite(state.limit_qd) || state.limit_qd <= 0.0) {
+        return true;
+    }
+
+    const auto velocity_limit =
+        static_cast<std::uint32_t>(std::abs(radPerSecToCounts_(state.limit_qd, state)));
+    const bool ok =
+        bus_runtime_->writeSdo<std::uint32_t>(node_id, msgs::CIA402_MAX_PROFILE_VELOCITY_607F,
+                                  0x00, velocity_limit);
+    if (!ok) {
+        CMVR_LOG(ERROR) << "[Cia402Protocol] failed to write velocity limit "
+                        << "to dictionary, node=" << static_cast<int>(node_id)
+                        << ", velocity_limit=" << velocity_limit;
+    }
+    return ok;
+}
+
+bool Cia402Protocol::writeAccelerationLimitsToDictionary_(
+    const std::uint8_t node_id,
+    const NodeState& state) const
+{
+    if (!bus_runtime_) {
+        return false;
+    }
+    if (!std::isfinite(state.limit_qdd) || state.limit_qdd <= 0.0) {
+        return true;
+    }
+
+    const auto acceleration_limit =
+        static_cast<std::uint32_t>(std::abs(radPerSec2ToCounts_(state.limit_qdd, state)));
+    const bool ok =
+        bus_runtime_->writeSdo<std::uint32_t>(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083,
+                                  0x00, acceleration_limit) &&
+        bus_runtime_->writeSdo<std::uint32_t>(node_id, msgs::CIA402_PROFILE_DECELERATION_6084,
+                                  0x00, acceleration_limit);
+    if (!ok) {
+        CMVR_LOG(ERROR) << "[Cia402Protocol] failed to write acceleration limits "
+                        << "to dictionary, node=" << static_cast<int>(node_id)
+                        << ", acceleration_limit=" << acceleration_limit;
+    }
+    return ok;
+}
+
+void Cia402Protocol::writeProfilePositionTarget_(const std::uint8_t node_id,
+                                                         NodeState& state)
+{
+    if (!bus_runtime_) {
+        return;
+    }
+
+    if (state.profile_velocity <= 0 && state.limit_qd > 0.0) {
+        state.profile_velocity = std::abs(radPerSecToCounts_(state.limit_qd, state));
+    }
+    if (state.profile_acceleration <= 0 && state.limit_qdd > 0.0) {
+        const auto profile_acceleration = std::abs(radPerSec2ToCounts_(state.limit_qdd, state));
+        state.profile_acceleration = profile_acceleration;
+        state.profile_deceleration = profile_acceleration;
+    }
+
+    state.controlword = cia402::enableOperationControlword();
+    writeNode_(node_id, state);
+
+    state.controlword = cia402::profilePositionControlword(true);
+    writeControlword_(node_id, state.controlword);
+    std::this_thread::sleep_for(
+        std::chrono::milliseconds(config_.profile_position_trigger_delay_ms()));
+
+    state.controlword = cia402::profilePositionControlword(false);
+    writeControlword_(node_id, state.controlword);
+}
+
+bool Cia402Protocol::prepareSafeTargetsForMode_(const std::uint8_t node_id,
+                                                        const msgs::RunMode mode,
+                                                        NodeState& state)
+{
+    state.target_velocity = 0;
+    state.target_torque = 0;
+
+    switch (mode) {
+        case msgs::RUN_MODE_PROFILE_POSITION:
+        case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: {
+            std::int32_t actual_position = 0;
+            if (!readActualPosition_(node_id, actual_position)) {
+                CMVR_LOG(ERROR) << "[Cia402Protocol] failed to read actual "
+                                << "position before switching mode, node="
+                                << static_cast<int>(node_id)
+                                << ", mode=" << static_cast<int>(mode);
+                return false;
+            }
+            state.target_position = actual_position;
+            break;
+        }
+
+        case msgs::RUN_MODE_PROFILE_VELOCITY:
+        case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY:
+        case msgs::RUN_MODE_CYCLIC_SYNC_CURRENT:
+        case msgs::RUN_MODE_HOMING:
+        case msgs::RUN_MODE_UNSPECIFIED:
+        default:
+            break;
+    }
+
+    return true;
+}
+
+void Cia402Protocol::writeTargetsForMode_(const std::uint8_t node_id,
+                                                  const msgs::RunMode mode,
+                                                  const NodeState& state) const
+{
+    if (!bus_runtime_) {
+        return;
+    }
+
+    switch (mode) {
+        case msgs::RUN_MODE_PROFILE_POSITION:
+            bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_TARGET_POSITION_607A,
+                                      0x00, state.target_position);
+            if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_VELOCITY_6081, 0x00)) {
+                bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_PROFILE_VELOCITY_6081,
+                                          0x00, state.profile_velocity);
+            }
+            if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00)) {
+                bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083,
+                                          0x00, state.profile_acceleration);
+            }
+            if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, 0x00)) {
+                bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_PROFILE_DECELERATION_6084,
+                                          0x00, state.profile_deceleration);
+            }
+            break;
+
+        case msgs::RUN_MODE_PROFILE_VELOCITY:
+            bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_TARGET_VELOCITY_60FF,
+                                      0x00, state.target_velocity);
+            if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00)) {
+                bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083,
+                                          0x00, state.profile_acceleration);
+            }
+            if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, 0x00)) {
+                bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_PROFILE_DECELERATION_6084,
+                                          0x00, state.profile_deceleration);
+            }
+            break;
+
+        case msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
+            bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_TARGET_POSITION_607A,
+                                      0x00, state.target_position);
+            bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_TARGET_VELOCITY_60FF,
+                                      0x00, state.target_velocity);
+            break;
+
+        case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY:
+            bus_runtime_->writePdo<std::int32_t>(node_id, msgs::CIA402_TARGET_VELOCITY_60FF,
+                                      0x00, state.target_velocity);
+            break;
+
+        case msgs::RUN_MODE_CYCLIC_SYNC_CURRENT:
+            bus_runtime_->writePdo<std::int16_t>(node_id, msgs::CIA402_TARGET_TORQUE_6071,
+                                      0x00, state.target_torque);
+            break;
+
+        default:
+            break;
+    }
+}
+
+void Cia402Protocol::writeNode_(const std::uint8_t node_id, NodeState& state)
+{
+    if (!bus_runtime_) {
+        return;
+    }
+
+    std::uint16_t statusword = 0;
+    if (readStatusword_(node_id, statusword)) {
+        const auto status = cia402::statusword(statusword);
+        state.controlword = nextControlword_(status);
+        std::int32_t actual_position = 0;
+        if (!isOperationEnabled_(status) &&
+            readActualPosition_(node_id, actual_position) &&
+            actual_position != 0) {
+            state.target_position = actual_position;
+        }
+    }
+
+    bus_runtime_->writePdo<std::uint16_t>(node_id, msgs::CIA402_CONTROL_WORD_6040, 0x00,
+                                          state.controlword.value);
+    bus_runtime_->writePdo<std::int8_t>(node_id, msgs::CIA402_OPERATION_MODE_6060, 0x00,
+                             toCia402Mode_(state.mode));
+
+    writeTargetsForMode_(node_id, state.mode, state);
+}
+
+} // namespace cmvr::device

cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp

@@ -0,0 +1,174 @@
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h"
+
+#include <cmath>
+#include <cstdint>
+#include <utility>
+
+#include "common/base/logging/logger.h"
+
+namespace cmvr::device {
+
+EyouMotor::EyouMotor(const config::MotorConfigItem& config,
+                     std::shared_ptr<Cia402Protocol> cia402_protocol,
+                     std::unique_ptr<EyouMotorAdapter> vendor_adapter)
+    : cia402_protocol_(std::move(cia402_protocol)),
+      vendor_adapter_(std::move(vendor_adapter))
+{
+    info_.id = config.id();
+    info_.joint_name = config.joint_name();
+    info_.limit_q_lb = config.limit_q_lb();
+    info_.limit_q_ub = config.limit_q_ub();
+    info_.limit_qd = config.limit_qd();
+    info_.limit_qdd = config.limit_qdd();
+    encoder_counts_per_rev_ = config.encoder_counts_per_rev();
+    gear_ratio_ = config.gear_ratio();
+    node_id_ = static_cast<std::uint8_t>(info_.id);
+    id_ = info_.joint_name;
+    protocol_ = cia402_protocol_;
+}
+
+bool EyouMotor::init()
+{
+    std::scoped_lock lock(mtx_);
+    if (!hasDependencies_() || !hasValidConversion_()) {
+        return false;
+    }
+
+    if (!cia402_protocol_->initNode(node_id_)) {
+        CMVR_LOG(ERROR) << "[EyouMotor] failed to init CiA402 node: " << info_.joint_name;
+        return false;
+    }
+    if (!vendor_adapter_->initNode(node_id_)) {
+        CMVR_LOG(ERROR) << "[EyouMotor] failed to init vendor adapter: " << info_.joint_name;
+        return false;
+    }
+    cia402_protocol_->setMotorConversion(node_id_, encoder_counts_per_rev_, gear_ratio_);
+
+    cia402_protocol_->setLimitQd(node_id_, info_.limit_qd);
+    if (!writeVendorVelocityLimit_()) {
+        return false;
+    }
+    if (info_.limit_qdd > 0.0) {
+        cia402_protocol_->setLimitQdd(node_id_, info_.limit_qdd, -info_.limit_qdd);
+    }
+    if (!writeVendorPositionLimits_()) {
+        return false;
+    }
+    return true;
+}
+
+void EyouMotor::setLimitQ(const double ub, const double lb)
+{
+    std::scoped_lock lock(mtx_);
+    info_.limit_q_ub = ub;
+    info_.limit_q_lb = lb;
+    if (!hasDependencies_() || !hasValidConversion_()) {
+        return;
+    }
+    writeVendorPositionLimits_();
+}
+
+void EyouMotor::setLimitQd(const double qd)
+{
+    std::scoped_lock lock(mtx_);
+    info_.limit_qd = qd;
+    if (!hasDependencies_() || !hasValidConversion_()) {
+        return;
+    }
+    cia402_protocol_->setLimitQd(node_id_, info_.limit_qd);
+    writeVendorVelocityLimit_();
+}
+
+bool EyouMotor::calibrateZeroQ()
+{
+    std::scoped_lock lock(mtx_);
+    if (!hasDependencies_() || !hasValidConversion_()) {
+        return false;
+    }
+
+    std::int32_t zeroed_position = 0;
+    if (!vendor_adapter_->calibrateZero(node_id_, zeroed_position)) {
+        CMVR_LOG(ERROR) << "[EyouMotor] zero calibration failed: " << info_.joint_name;
+        return false;
+    }
+    if (!cia402_protocol_->syncTargetToActualPosition(node_id_)) {
+        return false;
+    }
+    if (!writeVendorPositionLimits_()) {
+        return false;
+    }
+    return true;
+}
+
+bool EyouMotor::brakeRelease()
+{
+    std::scoped_lock lock(mtx_);
+    if (!hasDependencies_()) {
+        return false;
+    }
+    return vendor_adapter_->brakeRelease(node_id_);
+}
+
+bool EyouMotor::hasDependencies_() const
+{
+    if (!cia402_protocol_ || !vendor_adapter_) {
+        CMVR_LOG(ERROR) << "[EyouMotor] missing protocol or vendor adapter: "
+                        << info_.joint_name;
+        return false;
+    }
+    if (cia402_protocol_->comm_proto != MotorProtocolInterface::CommProto::ETHERCAT) {
+        CMVR_LOG(ERROR) << "[EyouMotor] invalid protocol for motor: " << info_.joint_name;
+        return false;
+    }
+    return true;
+}
+
+bool EyouMotor::hasValidConversion_() const
+{
+    if (encoder_counts_per_rev_ > 0.0 && gear_ratio_ > 0.0) {
+        return true;
+    }
+    CMVR_LOG(ERROR) << "[EyouMotor] missing encoder conversion config: "
+                    << info_.joint_name
+                    << ", encoder_counts_per_rev=" << encoder_counts_per_rev_
+                    << ", gear_ratio=" << gear_ratio_;
+    return false;
+}
+
+bool EyouMotor::writeVendorPositionLimits_() const
+{
+    if (!std::isfinite(info_.limit_q_lb) || !std::isfinite(info_.limit_q_ub) ||
+        info_.limit_q_ub <= info_.limit_q_lb) {
+        return true;
+    }
+
+    cia402_protocol_->setLimitQ(node_id_, info_.limit_q_ub, info_.limit_q_lb);
+    return vendor_adapter_->writePositionLimits(node_id_,
+                                                radToCounts_(info_.limit_q_lb),
+                                                radToCounts_(info_.limit_q_ub));
+}
+
+bool EyouMotor::writeVendorVelocityLimit_() const
+{
+    if (!std::isfinite(info_.limit_qd) || info_.limit_qd <= 0.0) {
+        return true;
+    }
+
+    return vendor_adapter_->writeVelocityLimit(node_id_, radPerSecToCounts_(info_.limit_qd));
+}
+
+std::int32_t EyouMotor::radToCounts_(const double angle_rad) const
+{
+    const double rev = angle_rad / (2.0 * M_PI);
+    return static_cast<std::int32_t>(
+        std::llround(rev * gear_ratio_ * encoder_counts_per_rev_));
+}
+
+std::uint32_t EyouMotor::radPerSecToCounts_(const double velocity_rad_s) const
+{
+    const double rev_per_sec = std::abs(velocity_rad_s) / (2.0 * M_PI);
+    return static_cast<std::uint32_t>(
+        std::llround(rev_per_sec * gear_ratio_ * encoder_counts_per_rev_));
+}
+
+} // namespace cmvr::device

cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp

@@ -0,0 +1,221 @@
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h"
+
+#include <chrono>
+#include <cmath>
+#include <cstdlib>
+#include <limits>
+#include <thread>
+#include <utility>
+
+#include "cmvr/msgs/cia402.pb.h"
+#include "common/base/logging/logger.h"
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h"
+
+namespace cmvr::device {
+
+EyouMotorAdapter::EyouMotorAdapter(
+    std::shared_ptr<EthercatMotorBusRuntime> bus_runtime)
+    : bus_runtime_(std::move(bus_runtime))
+{
+}
+
+bool EyouMotorAdapter::initNode(const std::uint8_t node_id)
+{
+    return bus_runtime_ && bus_runtime_->hasMotor(node_id);
+}
+
+bool EyouMotorAdapter::writePositionLimits(const std::uint8_t node_id,
+                                            const std::int32_t lower_limit,
+                                            const std::int32_t upper_limit)
+{
+    if (!bus_runtime_) {
+        return false;
+    }
+
+    const auto write_limits = [&]() {
+        return bus_runtime_->writeSdo<std::uint32_t>(node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
+                                                     0x00, 0) &&
+               bus_runtime_->writeSdo<std::int32_t>(
+                   node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D,
+                   0x02, upper_limit) &&
+               bus_runtime_->writeSdo<std::int32_t>(
+                   node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D,
+                   0x01, lower_limit) &&
+               bus_runtime_->writeSdo<std::uint32_t>(
+                   node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
+                   0x00, 0x4C494D54);
+    };
+    const auto readback_matches = [&]() {
+        std::int32_t actual_lower = 0;
+        std::int32_t actual_upper = 0;
+        return bus_runtime_->readSdo<std::int32_t>(
+                   node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D,
+                   0x01, actual_lower) &&
+               bus_runtime_->readSdo<std::int32_t>(
+                   node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D,
+                   0x02, actual_upper) &&
+               actual_lower == lower_limit &&
+               actual_upper == upper_limit;
+    };
+
+    const bool ok = write_limits() && readback_matches();
+    if (!ok) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write software position "
+                        << "limits, node=" << static_cast<int>(node_id)
+                        << ", soft_limit_state=" << 0x4C494D54
+                        << ", lower=" << lower_limit
+                        << ", upper=" << upper_limit;
+    }
+    return ok;
+}
+
+bool EyouMotorAdapter::writeVelocityLimit(const std::uint8_t node_id,
+                                           const std::uint32_t velocity_limit)
+{
+    if (!bus_runtime_) {
+        return false;
+    }
+
+    std::uint32_t actual_velocity_limit = 0;
+    const bool ok =
+        bus_runtime_->writeSdo<std::uint32_t>(node_id, eyou::EYOU_OVER_SPEED_THRESHOLD_2024,
+                                              0x00, velocity_limit) &&
+        bus_runtime_->readSdo<std::uint32_t>(node_id, eyou::EYOU_OVER_SPEED_THRESHOLD_2024,
+                                             0x00, actual_velocity_limit) &&
+        actual_velocity_limit == velocity_limit;
+    if (!ok) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write over speed "
+                        << "threshold, node=" << static_cast<int>(node_id)
+                        << ", expected=" << velocity_limit
+                        << ", actual=" << actual_velocity_limit;
+    }
+    return ok;
+}
+
+bool EyouMotorAdapter::calibrateZero(const std::uint8_t node_id,
+                                      std::int32_t& zeroed_position)
+{
+    if (!bus_runtime_) {
+        return false;
+    }
+
+    if (!bus_runtime_->writeSdo<std::uint32_t>(node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
+                                               0x00, 0)) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to disable software position "
+                        << "limit before home offset calibration, node="
+                        << static_cast<int>(node_id);
+        return false;
+    }
+
+    if (!bus_runtime_->writeSdo<std::int32_t>(node_id, msgs::CIA402_HOME_OFFSET_607C,
+                                              0x00, 0)) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to clear home offset, node="
+                        << static_cast<int>(node_id);
+        return false;
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds{50});
+
+    std::int32_t actual_position = 0;
+    if (!bus_runtime_->readSdo<std::int32_t>(node_id, msgs::CIA402_ACTUAL_POSITION_6064,
+                                             0x00, actual_position)) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read actual position "
+                        << "after clearing home offset, node="
+                        << static_cast<int>(node_id);
+        return false;
+    }
+
+    if (actual_position == std::numeric_limits<std::int32_t>::min()) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] invalid actual position for home "
+                        << "offset calibration, node=" << static_cast<int>(node_id)
+                        << ", actual_position=" << actual_position;
+        return false;
+    }
+
+    const auto home_offset = static_cast<std::int32_t>(-actual_position);
+    if (!bus_runtime_->writeSdo<std::int32_t>(node_id, msgs::CIA402_HOME_OFFSET_607C,
+                                              0x00, home_offset)) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write home offset, node="
+                        << static_cast<int>(node_id)
+                        << ", home_offset=" << home_offset;
+        return false;
+    }
+
+    if (!bus_runtime_->writeSdo<std::uint32_t>(
+            node_id, eyou::EYOU_STORE_PARAMETERS_1010,
+            0x01,
+            0x65766173)) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to save home offset parameter, node="
+                        << static_cast<int>(node_id);
+        return false;
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds{50});
+
+    std::int32_t home_offset_readback = 0;
+    if (!bus_runtime_->readSdo<std::int32_t>(node_id, msgs::CIA402_HOME_OFFSET_607C,
+                                             0x00, home_offset_readback)) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read back home offset, node="
+                        << static_cast<int>(node_id);
+        return false;
+    }
+    if (home_offset_readback != home_offset) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] home offset readback mismatch, node="
+                        << static_cast<int>(node_id)
+                        << ", expected=" << home_offset
+                        << ", actual=" << home_offset_readback;
+        return false;
+    }
+
+    if (!bus_runtime_->readSdo<std::int32_t>(node_id, msgs::CIA402_ACTUAL_POSITION_6064,
+                                             0x00, zeroed_position)) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read actual position "
+                        << "after writing home offset, node="
+                        << static_cast<int>(node_id);
+        return false;
+    }
+    if (std::abs(static_cast<long long>(zeroed_position)) > 10000) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] home offset did not zero actual position, "
+                        << "node=" << static_cast<int>(node_id)
+                        << ", actual_position_before=" << actual_position
+                        << ", home_offset=" << home_offset
+                        << ", home_offset_readback=" << home_offset_readback
+                        << ", actual_position_after=" << zeroed_position
+                        << ", tolerance_counts=" << 10000;
+        return false;
+    }
+
+    return true;
+}
+
+bool EyouMotorAdapter::brakeRelease(const std::uint8_t node_id)
+{
+    if (!bus_runtime_) {
+        return false;
+    }
+    if (!bus_runtime_->writeSdo<std::uint8_t>(
+            node_id, eyou::EYOU_BRAKE_CONTROL_2014,
+            0x01,
+            1)) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to release brake, node="
+                        << static_cast<int>(node_id);
+        return false;
+    }
+
+    const auto deadline =
+        std::chrono::steady_clock::now() + std::chrono::milliseconds{1000};
+    do {
+        std::uint8_t brake_state = 0;
+        if (bus_runtime_->readSdo<std::uint8_t>(
+                node_id, eyou::EYOU_BRAKE_CONTROL_2014,
+                0x02, brake_state) &&
+            (brake_state == 1 || brake_state == 2)) {
+            return true;
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds{10});
+    } while (std::chrono::steady_clock::now() < deadline);
+
+    CMVR_LOG(ERROR) << "[EyouMotorAdapter] brake release timeout, node="
+                    << static_cast<int>(node_id);
+    return false;
+}
+
+} // namespace cmvr::device

cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp

@@ -0,0 +1,309 @@
+#include <array>
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <cstdint>
+#include <iostream>
+#include <limits>
+#include <memory>
+#include <thread>
+
+#include <gtest/gtest.h>
+
+#include "cmvr/config/device_manager_config/device_manager_config.pb.h"
+#include "common/config/config_files.h"
+#include "devices/motor/abstract_motor.h"
+#include "devices/motor/manager/include/motor_manager.h"
+#include "manager/device_manager/include/device_manager.h"
+
+namespace cmvr::device {
+namespace {
+
+constexpr const char* kMotorManagerId = "ethercat_motors";
+constexpr const char* kMotorConfigFile =
+    "devices/motor/ethercat_motors_two_real_test.pb.txt";
+constexpr std::array<int, 4> kFourMotorIds{1, 2, 3, 4};
+constexpr std::chrono::milliseconds kCyclicCommandPeriod{1};
+constexpr std::chrono::milliseconds kPrintPeriod{100};
+constexpr std::chrono::milliseconds kStatsSamplePeriod{10};
+constexpr std::chrono::milliseconds kHoldAfterTrajectoryDuration{500};
+constexpr std::chrono::milliseconds kFourMotorTrajectoryDuration{20000};
+constexpr double kPi = 3.14159265358979323846;
+constexpr double kFourMotorAmplitudeRad = 0.2;
+constexpr double kFourMotorPeriodS = 1.0;
+constexpr std::array<double, 4> kFourMotorPhaseRad{0.0, 0.0, 0.0, 0.0};
+
+class DeviceManagerDestroyGuard {
+public:
+    ~DeviceManagerDestroyGuard()
+    {
+        DeviceManager::destroyInstance();
+    }
+};
+
+struct TrackingErrorStats {
+    std::int64_t sample_count{0};
+    double sum_error{0.0};
+    double sum_error_sq{0.0};
+    double max_abs_error{0.0};
+    double sin_projection{0.0};
+    double cos_projection{0.0};
+
+    void add(const double error, const double theta)
+    {
+        ++sample_count;
+        sum_error += error;
+        sum_error_sq += error * error;
+        max_abs_error = std::max(max_abs_error, std::fabs(error));
+        sin_projection += error * std::sin(theta);
+        cos_projection += error * std::cos(theta);
+    }
+
+    double mean() const
+    {
+        return sample_count > 0 ? sum_error / static_cast<double>(sample_count) : 0.0;
+    }
+
+    double rms() const
+    {
+        return sample_count > 0
+                   ? std::sqrt(sum_error_sq / static_cast<double>(sample_count))
+                   : 0.0;
+    }
+
+    double fundamentalAmplitude() const
+    {
+        if (sample_count == 0) {
+            return 0.0;
+        }
+        const double scale = 2.0 / static_cast<double>(sample_count);
+        return scale * std::sqrt(sin_projection * sin_projection +
+                                 cos_projection * cos_projection);
+    }
+
+    double phaseRad() const
+    {
+        return std::atan2(cos_projection, sin_projection);
+    }
+};
+
+double normalizePhaseRad(double phase)
+{
+    while (phase > kPi) {
+        phase -= 2.0 * kPi;
+    }
+    while (phase < -kPi) {
+        phase += 2.0 * kPi;
+    }
+    return phase;
+}
+
+double radToDeg(const double rad)
+{
+    return rad * 180.0 / kPi;
+}
+
+config::DeviceManagerConfig createEthercatOnlyDeviceManagerConfig()
+{
+    config::DeviceManagerConfig config;
+    config.set_name("eyou_motor_device_manager_real_test");
+    config.set_version("test");
+    config.set_init_all_motors_when_no_active_joints(true);
+
+    auto* motor_entry = config.add_devices();
+    motor_entry->set_id(kMotorManagerId);
+    motor_entry->set_type(config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM);
+    motor_entry->set_config_file(kMotorConfigFile);
+    motor_entry->set_enable(true);
+
+    return config;
+}
+
+void printMotorState(const int motor_id, const std::shared_ptr<AbstractMotor>& motor)
+{
+    ASSERT_NE(motor, nullptr);
+    std::cout << "motor_id=" << motor_id
+              << ", joint_name=" << motor->jointName()
+              << ", q=" << motor->getQ() << " rad"
+              << ", qd=" << motor->getQd() << " rad/s"
+              << std::endl;
+}
+
+} // namespace
+
+TEST(EyouMotorDeviceManagerRealTest, InitFourEthercatMotorsAndPrintState)
+{
+    ConfigHelper::setConfigRootFromFile("cmvr-es/config/cmvr_es.pb.txt");
+    DeviceManagerDestroyGuard guard;
+
+    auto& device_manager =
+        DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig());
+    auto motor_manager = device_manager.getDevice<MotorManager>(kMotorManagerId);
+    ASSERT_NE(motor_manager, nullptr);
+
+    // for (int motor_id = 1; motor_id <= 4; ++motor_id) {
+    //     printMotorState(motor_id, motor_manager->getMotor(motor_id));
+    // }
+
+    auto motor = motor_manager->getMotor(4);
+    motor->calibrateZeroQ();
+
+    ASSERT_TRUE(motor->torqueOn());
+    motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY);
+    motor->commandProfileVelocity(-2,5);
+
+
+    for (int i = 1; i <= 50; ++i)
+    {
+        auto q = motor->getQ();
+        auto qd = motor->getQd();
+        std::cout << "q=" << q << ", qd=" << qd << std::endl;
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+    motor->quickStop();
+
+}
+
+TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
+{
+    ConfigHelper::setConfigRootFromFile("cmvr-es/config/cmvr_es.pb.txt");
+    DeviceManagerDestroyGuard guard;
+
+    auto& device_manager =
+        DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig());
+    auto motor_manager = device_manager.getDevice<MotorManager>(kMotorManagerId);
+    ASSERT_NE(motor_manager, nullptr);
+
+    std::array<std::shared_ptr<AbstractMotor>, kFourMotorIds.size()> motors;
+    for (std::size_t i = 0; i < kFourMotorIds.size(); ++i) {
+        const int motor_id = kFourMotorIds[i];
+        motors[i] = motor_manager->getMotor(static_cast<std::uint8_t>(motor_id));
+        ASSERT_NE(motors[i], nullptr);
+        printMotorState(motor_id, motors[i]);
+    }
+
+    std::cout << "calibrate zero for four EtherCAT motors" << std::endl;
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        const int motor_id = kFourMotorIds[i];
+        std::cout << "before calibrateZeroQ: ";
+        printMotorState(motor_id, motors[i]);
+        ASSERT_TRUE(motors[i]->calibrateZeroQ());
+        std::cout << "after calibrateZeroQ: ";
+        printMotorState(motor_id, motors[i]);
+    }
+
+    for (const auto& motor : motors) {
+        ASSERT_TRUE(motor->torqueOn());
+    }
+    for (const auto& motor : motors) {
+        motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
+    }
+
+    std::array<double, kFourMotorIds.size()> center_q{};
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        center_q[i] = motors[i]->getQ();
+    }
+
+    const double omega = 2.0 * kPi / kFourMotorPeriodS;
+    std::cout << "command four motors in CSP, duration="
+              << kFourMotorTrajectoryDuration.count()
+              << " ms, command_period=" << kCyclicCommandPeriod.count()
+              << " ms, amplitude=" << kFourMotorAmplitudeRad
+              << " rad, period=" << kFourMotorPeriodS
+              << " s" << std::endl;
+
+    const auto start_time = std::chrono::steady_clock::now();
+    const auto total_ticks = kFourMotorTrajectoryDuration / kCyclicCommandPeriod;
+    std::array<TrackingErrorStats, kFourMotorIds.size()> error_stats;
+    double max_error_spread_rad = 0.0;
+    double sum_error_spread_sq = 0.0;
+    std::int64_t error_spread_sample_count = 0;
+    for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
+        const auto elapsed = tick * kCyclicCommandPeriod;
+        const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
+
+        std::array<double, kFourMotorIds.size()> target_q{};
+        std::array<double, kFourMotorIds.size()> target_qd{};
+        for (std::size_t i = 0; i < motors.size(); ++i) {
+            const double theta = omega * t_s + kFourMotorPhaseRad[i];
+            target_q[i] = center_q[i] + kFourMotorAmplitudeRad * (1.0 - std::cos(theta));
+            target_qd[i] = kFourMotorAmplitudeRad * omega * std::sin(theta);
+            ASSERT_TRUE(motors[i]->commandCyclicPosition(target_q[i], target_qd[i]));
+        }
+
+        if (elapsed.count() % kStatsSamplePeriod.count() == 0) {
+            double min_error = std::numeric_limits<double>::max();
+            double max_error = std::numeric_limits<double>::lowest();
+            for (std::size_t i = 0; i < motors.size(); ++i) {
+                const double theta = omega * t_s + kFourMotorPhaseRad[i];
+                const double error = motors[i]->getQ() - target_q[i];
+                error_stats[i].add(error, theta);
+                min_error = std::min(min_error, error);
+                max_error = std::max(max_error, error);
+            }
+            const double error_spread = max_error - min_error;
+            max_error_spread_rad = std::max(max_error_spread_rad, error_spread);
+            sum_error_spread_sq += error_spread * error_spread;
+            ++error_spread_sample_count;
+        }
+
+        // if (elapsed.count() % kPrintPeriod.count() == 0) {
+        //     std::cout << "t=" << elapsed.count() << " ms" << std::endl;
+        //     for (std::size_t i = 0; i < motors.size(); ++i) {
+        //         std::cout << "  motor_id=" << kFourMotorIds[i]
+        //                   << ", target_q=" << target_q[i]
+        //                   << " rad, target_qd=" << target_qd[i]
+        //                   << " rad/s, q=" << motors[i]->getQ()
+        //                   << " rad, qd=" << motors[i]->getQd()
+        //                   << " rad/s" << std::endl;
+        //     }
+        // }
+
+        std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
+    }
+
+    const auto hold_start_time = std::chrono::steady_clock::now();
+    const auto hold_ticks = kHoldAfterTrajectoryDuration / kCyclicCommandPeriod;
+    for (std::int64_t tick = 0; tick <= hold_ticks; ++tick) {
+        for (std::size_t i = 0; i < motors.size(); ++i) {
+            ASSERT_TRUE(motors[i]->commandCyclicPosition(center_q[i], 0.0));
+        }
+        std::this_thread::sleep_until(hold_start_time + (tick + 1) * kCyclicCommandPeriod);
+    }
+
+    std::cout << "after four motor CSP trajectory" << std::endl;
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        printMotorState(kFourMotorIds[i], motors[i]);
+    }
+
+    const double reference_phase = error_stats.front().phaseRad();
+    const double rms_error_spread =
+        error_spread_sample_count > 0
+            ? std::sqrt(sum_error_spread_sq / static_cast<double>(error_spread_sample_count))
+            : 0.0;
+    std::cout << "four motor CSP tracking error statistics, sample_period="
+              << kStatsSamplePeriod.count()
+              << " ms, samples=" << error_stats.front().sample_count
+              << ", max_error_spread=" << max_error_spread_rad
+              << " rad, rms_error_spread=" << rms_error_spread
+              << " rad" << std::endl;
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        const double phase = error_stats[i].phaseRad();
+        const double relative_phase = normalizePhaseRad(phase - reference_phase);
+        const double relative_phase_ms = relative_phase / omega * 1000.0;
+        std::cout << "  motor_id=" << kFourMotorIds[i]
+                  << ", mean_error=" << error_stats[i].mean()
+                  << " rad, rms_error=" << error_stats[i].rms()
+                  << " rad, max_abs_error=" << error_stats[i].max_abs_error
+                  << " rad, error_fundamental_amp="
+                  << error_stats[i].fundamentalAmplitude()
+                  << " rad, error_phase=" << phase
+                  << " rad (" << radToDeg(phase)
+                  << " deg), relative_phase_to_motor1=" << relative_phase
+                  << " rad (" << radToDeg(relative_phase)
+                  << " deg, " << relative_phase_ms
+                  << " ms)" << std::endl;
+    }
+}
+
+} // namespace cmvr::device

cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp

@@ -0,0 +1,559 @@
+#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
+
+#include <chrono>
+#include <cmath>
+#include <cstdint>
+#include <iomanip>
+#include <iostream>
+#include <memory>
+#include <sstream>
+#include <thread>
+#include <utility>
+
+#include <gtest/gtest.h>
+
+#include "cmvr/msgs/cia402.pb.h"
+#include "devices/motor/abstract_motor.h"
+#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h"
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h"
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h"
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h"
+
+namespace cmvr::device {
+namespace {
+
+constexpr int kMotorId = 1;
+constexpr std::chrono::milliseconds kModeSettleDelay{100};
+constexpr std::chrono::milliseconds kCommandSamplePeriod{100};
+constexpr std::chrono::milliseconds kCyclicCommandPeriod{1};
+constexpr std::chrono::milliseconds kFeedbackSampleDuration{5000};
+constexpr double kDefaultGearRatio = 101.0;
+constexpr double kEncoderCountsPerMotorRev = 65536.0;
+constexpr double kPi = 3.14159265358979323846;
+
+config::MotorGroupConfig createSingleSlaveGroup()
+{
+    config::MotorGroupConfig group;
+    group.set_id("eyou_motor_real_test");
+    group.set_bus_type(config::MOTOR_BUS_ETHERCAT);
+    group.set_vendor(config::MOTOR_VENDOR_EYOU);
+    group.set_protocol(config::MOTOR_PROTOCOL_ETHERCAT_CIA402);
+
+    auto* ethercat = group.mutable_ethercat();
+    ethercat->set_master_index(0);
+    ethercat->set_cycle_us(1000);
+    ethercat->set_slave_op_timeout_ms(12000);
+    ethercat->set_slave_state_poll_period_ms(10);
+
+    auto* cia402 = ethercat->mutable_cia402();
+    cia402->set_profile_position_trigger_delay_ms(2);
+    cia402->set_state_transition_timeout_ms(1200);
+    cia402->set_velocity_stop_timeout_ms(2000);
+    cia402->set_status_poll_period_ms(10);
+    cia402->set_stopped_velocity_tolerance_rad_s(0.001);
+
+    auto* dc = ethercat->mutable_dc();
+    dc->set_enable(true);
+    dc->set_reference_motor_id(kMotorId);
+    dc->set_sync0_cycle_us(1000);
+    dc->set_sync0_shift_us(0);
+    dc->set_sync_reference_clock_period(1);
+    dc->set_assign_activate(768);
+    dc->set_sync_monitor_period_ms(1000);
+
+    auto* slave = ethercat->add_slaves();
+    slave->set_motor_id(kMotorId);
+    slave->set_alias(0);
+    slave->set_position(0);
+
+    return group;
+}
+
+class RuntimeStopGuard {
+public:
+    explicit RuntimeStopGuard(std::shared_ptr<EthercatMotorBusRuntime> runtime)
+        : runtime_(std::move(runtime))
+    {
+    }
+
+    ~RuntimeStopGuard()
+    {
+        if (runtime_) {
+            runtime_->stop();
+        }
+    }
+
+private:
+    std::shared_ptr<EthercatMotorBusRuntime> runtime_;
+};
+
+std::shared_ptr<EthercatMotorBusRuntime> startRuntime()
+{
+    auto runtime = std::make_shared<EthercatMotorBusRuntime>();
+    runtime->setPdoMapping(createEyouCia402PdoMapping());
+    if (!runtime->init(createSingleSlaveGroup())) {
+        return nullptr;
+    }
+    if (!runtime->start()) {
+        runtime->stop();
+        return nullptr;
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+    return runtime;
+}
+
+std::shared_ptr<Cia402Protocol> createProtocol(
+    const std::shared_ptr<EthercatMotorBusRuntime>& runtime)
+{
+    return std::make_shared<Cia402Protocol>(runtime, runtime->config().cia402());
+}
+
+config::MotorConfigItem createMotorConfig()
+{
+    config::MotorConfigItem config;
+    config.set_id(kMotorId);
+    config.set_joint_name("ethercat_test_joint");
+    config.set_limit_q_lb(-6.14);
+    config.set_limit_q_ub(6.14);
+    config.set_limit_qd(10.0);
+    config.set_limit_qdd(10.0);
+    config.set_encoder_counts_per_rev(kEncoderCountsPerMotorRev);
+    config.set_gear_ratio(kDefaultGearRatio);
+    return config;
+}
+
+std::unique_ptr<AbstractMotor> createMotor(
+    const std::shared_ptr<EthercatMotorBusRuntime>& runtime)
+{
+    auto motor = std::make_unique<EyouMotor>(
+        createMotorConfig(),
+        createProtocol(runtime),
+        std::make_unique<EyouMotorAdapter>(runtime));
+    if (!motor->init()) {
+        return nullptr;
+    }
+    return motor;
+}
+
+void printMotorState(const char* label, AbstractMotor& motor)
+{
+    std::cout << label
+              << ": motor_q=" << motor.getQ() << " rad"
+              << ", motor_qd=" << motor.getQd() << " rad/s"
+              << std::endl;
+}
+
+std::string hex16(const std::uint16_t value)
+{
+    std::ostringstream oss;
+    oss << "0x" << std::uppercase << std::hex << std::setw(4) << std::setfill('0')
+        << value;
+    return oss.str();
+}
+
+void printRawEthercatFeedback(const char* label,
+                              const std::shared_ptr<EthercatMotorBusRuntime>& runtime)
+{
+    std::uint16_t statusword = 0;
+    std::int8_t mode_display = 0;
+    std::int32_t actual_position = 0;
+    std::int32_t actual_velocity = 0;
+    std::int16_t actual_torque = 0;
+    std::uint16_t error_code = 0;
+
+    runtime->readPdo<std::uint16_t>(kMotorId, msgs::CIA402_STATUS_WORD_6041, 0x00, statusword);
+    runtime->readPdo<std::int8_t>(kMotorId, msgs::CIA402_MODE_DISPLAY_6061, 0x00, mode_display);
+    runtime->readPdo<std::int32_t>(kMotorId, msgs::CIA402_ACTUAL_POSITION_6064, 0x00,
+                                   actual_position);
+    runtime->readPdo<std::int32_t>(kMotorId, msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00,
+                                   actual_velocity);
+    runtime->readPdo<std::int16_t>(kMotorId, msgs::CIA402_ACTUAL_TORQUE_6077, 0x00,
+                                   actual_torque);
+    runtime->readPdo<std::uint16_t>(kMotorId, msgs::CIA402_ERROR_CODE_603F, 0x00, error_code);
+
+    std::cout << label
+              << ": statusword=" << hex16(statusword)
+              << ", mode_display=" << static_cast<int>(mode_display)
+              << ", actual_position=" << actual_position
+              << ", actual_velocity=" << actual_velocity
+              << ", actual_torque=" << actual_torque
+              << ", error_code=" << hex16(error_code)
+              << std::endl;
+}
+
+void sampleMotorState(AbstractMotor& motor,
+                      const std::chrono::milliseconds duration)
+{
+    for (auto elapsed = std::chrono::milliseconds{0};
+         elapsed < duration;
+         elapsed += kCommandSamplePeriod) {
+        std::this_thread::sleep_for(kCommandSamplePeriod);
+        std::cout << "t=" << (elapsed + kCommandSamplePeriod).count() << " ms";
+        printMotorState("", motor);
+    }
+}
+
+double nearbySafeTarget(const double current_q, const double delta_rad)
+{
+    return current_q + (current_q > 0.0 ? -std::abs(delta_rad) : std::abs(delta_rad));
+}
+
+} // namespace
+
+TEST(EyouMotorRealTest, ReadMotorStateOnly)
+{
+    auto runtime = startRuntime();
+    ASSERT_NE(runtime, nullptr);
+    RuntimeStopGuard runtime_guard(runtime);
+
+    auto motor = createMotor(runtime);
+    ASSERT_NE(motor, nullptr);
+
+    ASSERT_TRUE(motor->torqueOn());
+    printMotorState("motor state", *motor);
+    printRawEthercatFeedback("raw feedback", runtime);
+    for (int i = 1; i <= 10; ++i) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+        printMotorState("motor state", *motor);
+        printRawEthercatFeedback("raw feedback", runtime);
+    }
+}
+
+TEST(EyouMotorRealTest, CalibrateZeroQPrintBeforeAndAfter)
+{
+    auto runtime = startRuntime();
+    ASSERT_NE(runtime, nullptr);
+    RuntimeStopGuard runtime_guard(runtime);
+
+    auto motor = createMotor(runtime);
+    ASSERT_NE(motor, nullptr);
+
+    ASSERT_TRUE(motor->torqueOff());
+    printMotorState("before calibrateZeroQ", *motor);
+    ASSERT_TRUE(motor->calibrateZeroQ());
+    printMotorState("after calibrateZeroQ", *motor);
+    ASSERT_TRUE(motor->torqueOn());
+}
+
+TEST(EyouMotorRealTest, CommandProfilePosition)
+{
+    auto runtime = startRuntime();
+    ASSERT_NE(runtime, nullptr);
+    RuntimeStopGuard runtime_guard(runtime);
+
+    auto motor = createMotor(runtime);
+    ASSERT_NE(motor, nullptr);
+
+    ASSERT_TRUE(motor->torqueOn());
+    motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
+    std::this_thread::sleep_for(kModeSettleDelay);
+
+    ASSERT_TRUE(motor->commandProfilePosition(-3.0, 0.5, 1.0));
+    sampleMotorState(*motor, kFeedbackSampleDuration);
+}
+
+TEST(EyouMotorRealTest, CommandProfileVelocity)
+{
+    auto runtime = startRuntime();
+    ASSERT_NE(runtime, nullptr);
+    RuntimeStopGuard runtime_guard(runtime);
+
+    auto motor = createMotor(runtime);
+    ASSERT_NE(motor, nullptr);
+
+    ASSERT_TRUE(motor->torqueOn());
+    motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY);
+    std::this_thread::sleep_for(kModeSettleDelay);
+
+    std::cout << "motor.commandProfileVelocity(0.3 rad/s, 1.0 rad/s^2)" << std::endl;
+    ASSERT_TRUE(motor->commandProfileVelocity(0.3, 1.0));
+    sampleMotorState(*motor, kFeedbackSampleDuration);
+
+    std::cout << "motor.commandProfileVelocity(0 rad/s, 1.0 rad/s^2)" << std::endl;
+    ASSERT_TRUE(motor->commandProfileVelocity(0.0, 1.0));
+    sampleMotorState(*motor, std::chrono::milliseconds{1000});
+}
+
+TEST(EyouMotorRealTest, CommandCyclicPosition)
+{
+    auto runtime = startRuntime();
+    ASSERT_NE(runtime, nullptr);
+    RuntimeStopGuard runtime_guard(runtime);
+
+    auto motor = createMotor(runtime);
+    ASSERT_NE(motor, nullptr);
+
+    ASSERT_TRUE(motor->torqueOn());
+    motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
+    std::this_thread::sleep_for(kModeSettleDelay);
+
+    const std::chrono::milliseconds trajectory_duration{15000};
+    const double period_s = 6.0;
+    const double amplitude_rad = 3;
+    const double phase_rad = 0.0;
+    const double center_q = motor->getQ();
+    const double omega = 2.0 * kPi / period_s;
+
+    std::cout << "motor.commandCyclicPosition(sin), center_q=" << center_q
+              << " rad, period=" << period_s
+              << " s, amplitude=" << amplitude_rad
+              << " rad, phase=" << phase_rad
+              << " rad, command_period=" << kCyclicCommandPeriod.count()
+              << " ms" << std::endl;
+
+    const auto start_time = std::chrono::steady_clock::now();
+    const auto total_ticks = trajectory_duration / kCyclicCommandPeriod;
+    for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
+        const auto elapsed = tick * kCyclicCommandPeriod;
+        const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
+        const double theta = omega * t_s + phase_rad;
+        const double target_q = center_q + amplitude_rad * std::sin(theta);
+        const double target_qd = amplitude_rad * omega * std::cos(theta);
+
+        ASSERT_TRUE(motor->commandCyclicPosition(target_q, target_qd));
+        if (elapsed.count() % kCommandSamplePeriod.count() == 0) {
+            std::cout << "t=" << elapsed.count()
+                      << " ms, target_q=" << target_q
+                      << " rad, target_qd=" << target_qd
+                      << " rad/s";
+            printMotorState("", *motor);
+        }
+        std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
+    }
+}
+
+TEST(EyouMotorRealTest, CommandCyclicVelocity)
+{
+    auto runtime = startRuntime();
+    ASSERT_NE(runtime, nullptr);
+    RuntimeStopGuard runtime_guard(runtime);
+
+    auto motor = createMotor(runtime);
+    ASSERT_NE(motor, nullptr);
+
+    ASSERT_TRUE(motor->torqueOn());
+    motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY);
+    std::this_thread::sleep_for(kModeSettleDelay);
+
+    const std::chrono::milliseconds trajectory_duration{15000};
+    const double period_s = 6.0;
+    const double velocity_amplitude_rad_s = 5.0;
+    const double phase_rad = 0.0;
+    const double omega = 2.0 * kPi / period_s;
+
+    std::cout << "motor.commandCyclicVelocity(sin), period=" << period_s
+              << " s, velocity_amplitude=" << velocity_amplitude_rad_s
+              << " rad/s, phase=" << phase_rad
+              << " rad, command_period=" << kCyclicCommandPeriod.count()
+              << " ms" << std::endl;
+
+    const auto start_time = std::chrono::steady_clock::now();
+    const auto total_ticks = trajectory_duration / kCyclicCommandPeriod;
+    for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
+        const auto elapsed = tick * kCyclicCommandPeriod;
+        const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
+        const double theta = omega * t_s + phase_rad;
+        const double target_qd = velocity_amplitude_rad_s * std::sin(theta);
+
+        ASSERT_TRUE(motor->commandCyclicVelocity(target_qd));
+        if (elapsed.count() % kCommandSamplePeriod.count() == 0) {
+            std::cout << "t=" << elapsed.count()
+                      << " ms, target_qd=" << target_qd
+                      << " rad/s";
+            printMotorState("", *motor);
+        }
+        std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
+    }
+
+    std::cout << "motor.commandCyclicVelocity(0 rad/s)" << std::endl;
+    ASSERT_TRUE(motor->commandCyclicVelocity(0.0));
+    sampleMotorState(*motor, std::chrono::milliseconds{500});
+}
+
+TEST(EyouMotorRealTest, QuickStopAfterTwoSeconds)
+{
+    auto runtime = startRuntime();
+    ASSERT_NE(runtime, nullptr);
+    RuntimeStopGuard runtime_guard(runtime);
+
+    auto motor = createMotor(runtime);
+    ASSERT_NE(motor, nullptr);
+
+    ASSERT_TRUE(motor->torqueOff());
+    ASSERT_TRUE(motor->calibrateZeroQ());
+    ASSERT_TRUE(motor->torqueOn());
+    motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY);
+    std::this_thread::sleep_for(kModeSettleDelay);
+
+    const std::chrono::milliseconds run_duration{2000};
+    const double period_s = 6.0;
+    const double velocity_amplitude_rad_s = 4.5;
+    const double phase_rad = 0.0;
+    const double omega = 2.0 * kPi / period_s;
+
+    std::cout << "motor.commandCyclicVelocity(sin), then quickStop at "
+              << run_duration.count()
+              << " ms, period=" << period_s
+              << " s, velocity_amplitude=" << velocity_amplitude_rad_s
+              << " rad/s, phase=" << phase_rad
+              << " rad, command_period=" << kCyclicCommandPeriod.count()
+              << " ms" << std::endl;
+
+    const auto start_time = std::chrono::steady_clock::now();
+    const auto total_ticks = run_duration / kCyclicCommandPeriod;
+    for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
+        const auto elapsed = tick * kCyclicCommandPeriod;
+        const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
+        const double theta = omega * t_s + phase_rad;
+        const double target_qd = velocity_amplitude_rad_s * std::sin(theta);
+
+        ASSERT_TRUE(motor->commandCyclicVelocity(target_qd));
+        if (elapsed.count() % kCommandSamplePeriod.count() == 0) {
+            std::cout << "t=" << elapsed.count()
+                      << " ms, target_qd=" << target_qd
+                      << " rad/s";
+            printMotorState("", *motor);
+            printRawEthercatFeedback("raw feedback", runtime);
+        }
+        std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
+    }
+
+    std::cout << "motor.quickStop()" << std::endl;
+    ASSERT_TRUE(motor->quickStop());
+    printMotorState("after quickStop", *motor);
+    printRawEthercatFeedback("raw feedback", runtime);
+    sampleMotorState(*motor, std::chrono::milliseconds{1000});
+    printRawEthercatFeedback("raw feedback", runtime);
+}
+
+TEST(EyouMotorRealTest, QuickStopInProfilePosition)
+{
+    auto runtime = startRuntime();
+    ASSERT_NE(runtime, nullptr);
+    RuntimeStopGuard runtime_guard(runtime);
+
+    auto motor = createMotor(runtime);
+    ASSERT_NE(motor, nullptr);
+
+    ASSERT_TRUE(motor->torqueOff());
+    ASSERT_TRUE(motor->calibrateZeroQ());
+    ASSERT_TRUE(motor->torqueOn());
+    motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
+    std::this_thread::sleep_for(kModeSettleDelay);
+
+    const std::chrono::milliseconds quick_stop_time{1000};
+    const double start_q = motor->getQ();
+    const double target_q = nearbySafeTarget(start_q, 4.0);
+    const double max_qd = 2.0;
+    const double max_qdd = 10.0;
+
+    std::cout << "motor.commandProfilePosition(" << target_q
+              << " rad, " << max_qd
+              << " rad/s, " << max_qdd
+              << " rad/s^2), then quickStop at "
+              << quick_stop_time.count() << " ms" << std::endl;
+    ASSERT_TRUE(motor->commandProfilePosition(target_q, max_qd, max_qdd));
+    std::cout << "wait " << quick_stop_time.count()
+              << " ms before quickStop" << std::endl;
+    sampleMotorState(*motor, quick_stop_time);
+    printRawEthercatFeedback("raw feedback before quickStop", runtime);
+
+    std::cout << "motor.quickStop()" << std::endl;
+    ASSERT_TRUE(motor->quickStop());
+    printMotorState("after quickStop", *motor);
+    printRawEthercatFeedback("raw feedback", runtime);
+    sampleMotorState(*motor, std::chrono::milliseconds{1000});
+    printRawEthercatFeedback("raw feedback", runtime);
+}
+
+TEST(EyouMotorRealTest, QuickStopInProfileVelocity)
+{
+    auto runtime = startRuntime();
+    ASSERT_NE(runtime, nullptr);
+    RuntimeStopGuard runtime_guard(runtime);
+
+    auto motor = createMotor(runtime);
+    ASSERT_NE(motor, nullptr);
+
+    ASSERT_TRUE(motor->torqueOff());
+    ASSERT_TRUE(motor->calibrateZeroQ());
+    ASSERT_TRUE(motor->torqueOn());
+    motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY);
+    std::this_thread::sleep_for(kModeSettleDelay);
+
+    const std::chrono::milliseconds quick_stop_time{2000};
+    const double target_qd = motor->getQ() > 0.0 ? -2.0 : 2.0;
+    const double max_qdd = 10.0;
+
+    std::cout << "motor.commandProfileVelocity(" << target_qd
+              << " rad/s, " << max_qdd
+              << " rad/s^2), then quickStop at "
+              << quick_stop_time.count() << " ms" << std::endl;
+    ASSERT_TRUE(motor->commandProfileVelocity(target_qd, max_qdd));
+    std::cout << "wait " << quick_stop_time.count()
+              << " ms before quickStop" << std::endl;
+    sampleMotorState(*motor, quick_stop_time);
+    printRawEthercatFeedback("raw feedback before quickStop", runtime);
+
+    std::cout << "motor.quickStop()" << std::endl;
+    ASSERT_TRUE(motor->quickStop());
+    printMotorState("after quickStop", *motor);
+    printRawEthercatFeedback("raw feedback", runtime);
+    sampleMotorState(*motor, std::chrono::milliseconds{1000});
+    printRawEthercatFeedback("raw feedback", runtime);
+}
+
+TEST(EyouMotorRealTest, QuickStopInCyclicPosition)
+{
+    auto runtime = startRuntime();
+    ASSERT_NE(runtime, nullptr);
+    RuntimeStopGuard runtime_guard(runtime);
+
+    auto motor = createMotor(runtime);
+    ASSERT_NE(motor, nullptr);
+
+    ASSERT_TRUE(motor->torqueOff());
+    ASSERT_TRUE(motor->calibrateZeroQ());
+    ASSERT_TRUE(motor->torqueOn());
+    motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
+    std::this_thread::sleep_for(kModeSettleDelay);
+
+    const std::chrono::milliseconds run_duration{2000};
+    const double start_q = motor->getQ();
+    const double target_qd = start_q > 0.0 ? -2.0 : 2.0;
+
+    std::cout << "motor.commandCyclicPosition(linear), start_q=" << start_q
+              << " rad, target_qd=" << target_qd
+              << " rad/s, then quickStop at "
+              << run_duration.count()
+              << " ms, command_period=" << kCyclicCommandPeriod.count()
+              << " ms" << std::endl;
+
+    const auto start_time = std::chrono::steady_clock::now();
+    const auto total_ticks = run_duration / kCyclicCommandPeriod;
+    for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
+        const auto elapsed = tick * kCyclicCommandPeriod;
+        const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
+        const double target_q = start_q + target_qd * t_s;
+
+        ASSERT_TRUE(motor->commandCyclicPosition(target_q, target_qd));
+        if (elapsed.count() % kCommandSamplePeriod.count() == 0) {
+            std::cout << "t=" << elapsed.count()
+                      << " ms, target_q=" << target_q
+                      << " rad, target_qd=" << target_qd
+                      << " rad/s";
+            printMotorState("", *motor);
+            printRawEthercatFeedback("raw feedback", runtime);
+        }
+        std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
+    }
+
+    std::cout << "motor.quickStop()" << std::endl;
+    ASSERT_TRUE(motor->quickStop());
+    printMotorState("after quickStop", *motor);
+    printRawEthercatFeedback("raw feedback", runtime);
+    sampleMotorState(*motor, std::chrono::milliseconds{1000});
+    printRawEthercatFeedback("raw feedback", runtime);
+}
+
+} // namespace cmvr::device

cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h

@@ -23,19 +23,25 @@ public:
 
     void setMode(msgs::RunMode mode) override;
     msgs::RunMode getMode() override;
-    void torqueOff() override;
+    bool torqueOn() override;
+    bool torqueOff() override;
+    bool brakeRelease() override;
+    bool quickStop() override;
 
     void setLimitQ(double ub, double lb) override;
     void setLimitQd(double qd) override;
     void setLimitQdd(double u_qdd, double l_qdd) override;
 
-    void brake() override;
-    void setQ(double q) override;
-    void setTarget(double q, double qd) override;
-    void setTarget(double qd) override;
     bool calibrateZeroQ() override;
     bool reachedTargetQ() override;
-    void setQd(double qd) override;
+    bool commandProfilePosition(double target_q,
+                                double max_qd = 0.0,
+                                double max_qdd = 0.0) override;
+    bool commandProfileVelocity(double target_qd, double max_qdd = 0.0) override;
+    bool commandCyclicPosition(double target_q,
+                               double target_qd = 0.0) override;
+    bool commandCyclicVelocity(double target_qd) override;
+    bool commandCyclicTorque(double target_tau) override;
     double getQ() override;
     double getQd() override;
 
@@ -44,6 +50,7 @@ public:
                                  const std::vector<double>& velocities);
 
 private:
+    bool holdPosition_();
     double clampQ_(double q) const;
     double clampQd_(double qd) const;
     std::shared_ptr<simulate::MujocoWorld> worldLocked_() const;

cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp

@@ -54,11 +54,27 @@ msgs::RunMode MujocoMotor::getMode()
     return mode_;
 }
 
-void MujocoMotor::torqueOff()
+bool MujocoMotor::torqueOn()
 {
-    brake();
+    return holdPosition_();
+}
+
+bool MujocoMotor::torqueOff()
+{
+    const bool ok = holdPosition_();
     std::scoped_lock lock(mtx_);
     mode_ = msgs::RUN_MODE_UNSPECIFIED;
+    return ok;
+}
+
+bool MujocoMotor::brakeRelease()
+{
+    return true;
+}
+
+bool MujocoMotor::quickStop()
+{
+    return holdPosition_();
 }
 
 void MujocoMotor::setLimitQ(const double ub, const double lb)
@@ -82,38 +98,78 @@ void MujocoMotor::setLimitQdd(const double u_qdd, const double l_qdd)
     info_.limit_qdd = std::max(limit_qdd_upper_, std::abs(limit_qdd_lower_));
 }
 
-void MujocoMotor::brake()
+bool MujocoMotor::holdPosition_()
 {
     const auto world = worldLocked_();
     double q = 0.0;
     if (!world || !world->getJointPosition(info_.joint_name, q)) {
-        return;
+        return false;
     }
     std::scoped_lock lock(mtx_);
     target_q_ = q;
     mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
     world->setJointTargetState(info_.joint_name, q, 0.0);
+    return true;
 }
 
-void MujocoMotor::setQ(const double q)
+bool MujocoMotor::commandProfilePosition(const double target_q,
+                                         const double max_qd,
+                                         const double max_qdd)
 {
-    setTarget(q, 0.0);
+    (void)max_qdd;
+    std::scoped_lock lock(mtx_);
+    const auto world = worldLocked_();
+    if (!world) {
+        return false;
+    }
+    mode_ = msgs::RUN_MODE_PROFILE_POSITION;
+    target_q_ = clampQ_(target_q);
+    const double profile_qd = max_qd > 0.0 ? max_qd : info_.limit_qd;
+    return world->setJointTargetState(info_.joint_name, target_q_, clampQd_(profile_qd));
 }
 
-void MujocoMotor::setTarget(const double q, const double qd)
+bool MujocoMotor::commandProfileVelocity(const double target_qd, const double max_qdd)
+{
+    (void)max_qdd;
+    std::scoped_lock lock(mtx_);
+    const auto world = worldLocked_();
+    if (!world) {
+        return false;
+    }
+    mode_ = msgs::RUN_MODE_PROFILE_VELOCITY;
+    return world->setJointTargetVelocity(info_.joint_name, clampQd_(target_qd));
+}
+
+bool MujocoMotor::commandCyclicPosition(const double target_q,
+                                        const double target_qd)
 {
     std::scoped_lock lock(mtx_);
     const auto world = worldLocked_();
     if (!world) {
-        return;
+        return false;
     }
-    target_q_ = clampQ_(q);
-    world->setJointTargetState(info_.joint_name, target_q_, clampQd_(qd));
+    mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
+    target_q_ = clampQ_(target_q);
+    return world->setJointTargetState(info_.joint_name, target_q_, clampQd_(target_qd));
 }
 
-void MujocoMotor::setTarget(const double qd)
+bool MujocoMotor::commandCyclicVelocity(const double target_qd)
 {
-    setQd(qd);
+    std::scoped_lock lock(mtx_);
+    const auto world = worldLocked_();
+    if (!world) {
+        return false;
+    }
+    mode_ = msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY;
+    return world->setJointTargetVelocity(info_.joint_name, clampQd_(target_qd));
+}
+
+bool MujocoMotor::commandCyclicTorque(const double target_tau)
+{
+    (void)target_tau;
+    CMVR_LOG(ERROR) << "[MujocoMotor] cyclic torque command is not implemented: "
+                    << info_.joint_name;
+    return false;
 }
 
 bool MujocoMotor::calibrateZeroQ()
@@ -140,16 +196,6 @@ bool MujocoMotor::reachedTargetQ()
     }
 }
 
-void MujocoMotor::setQd(const double qd)
-{
-    std::scoped_lock lock(mtx_);
-    const auto world = worldLocked_();
-    if (!world) {
-        return;
-    }
-    world->setJointTargetVelocity(info_.joint_name, clampQd_(qd));
-}
-
 double MujocoMotor::getQ()
 {
     const auto world = worldLocked_();

cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h

@@ -22,6 +22,8 @@ namespace  cmvr {
                 info_.limit_q_ub = config.limit_q_ub();
                 info_.limit_qd = config.limit_qd() > 0.0 ? config.limit_qd() : 0.5;
                 info_.limit_qdd = config.limit_qdd() > 0.0 ? config.limit_qdd() : 10.0;
+                encoder_counts_per_rev_ = config.encoder_counts_per_rev();
+                gear_ratio_ = config.gear_ratio();
                 node_id_ = info_.id;
             }
 
@@ -37,6 +39,19 @@ namespace  cmvr {
                 }
                 if (protocol_->comm_proto == MotorProtocolInterface::CommProto::CANOPEN ) {
                     auto canopen_protocol = std::dynamic_pointer_cast<Ti5MotorCanopenProtocol>(protocol_);
+                    if (!canopen_protocol) {
+                        CMVR_LOG(ERROR) << "[Ti5Motor] invalid CANopen protocol for motor: "
+                                        << info_.joint_name;
+                        return false;
+                    }
+                    if (encoder_counts_per_rev_ <= 0.0 || gear_ratio_ <= 0.0) {
+                        CMVR_LOG(ERROR) << "[Ti5Motor] missing encoder conversion config: "
+                                        << info_.joint_name
+                                        << ", encoder_counts_per_rev=" << encoder_counts_per_rev_
+                                        << ", gear_ratio=" << gear_ratio_;
+                        return false;
+                    }
+                    protocol_->setMotorConversion(node_id_, encoder_counts_per_rev_, gear_ratio_);
                     // torqueOff(node_id_);
                     canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_RESET_COMMUNICATION);
                     // canopen_protocol->torqueOff(node_id_);
@@ -45,8 +60,8 @@ namespace  cmvr {
                     canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_ENTER_PRE_OPERATIONAL);
                     canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_START_REMOTE_NODE);
                     canopen_protocol->setMode(node_id_,msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
-                    // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06,15);
-                    // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F,15);
+                    // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CIA402_CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06,15);
+                    // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CIA402_CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F,15);
                     canopen_protocol->setLimitQ(node_id_, info_.limit_q_ub, info_.limit_q_lb);
                     canopen_protocol->setLimitQd(node_id_, info_.limit_qd);
                     canopen_protocol->setLimitQdd(node_id_, info_.limit_qdd, -info_.limit_qdd);
@@ -55,6 +70,10 @@ namespace  cmvr {
                 }
                 return true;
             }
+
+        private:
+            double encoder_counts_per_rev_{0.0};
+            double gear_ratio_{0.0};
         };
 
 

cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h

@@ -18,6 +18,7 @@
 #include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo1.h"
 #include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo2.h"
 #include <cmath>
+#include <unordered_map>
 
 namespace cmvr {
     namespace device {
@@ -29,28 +30,40 @@ namespace cmvr {
 
             bool initNode(uint8_t node_id) override;
 
-            void setMode(uint8_t node_id, msgs::RunMode mode);
-            void setTarget(uint8_t node_id, double angle_rad, double vel) override;
-            void setTarget(uint8_t node_id, double vel) override;
-            void setQ(uint8_t node_id, double angle_rad) override;
+            void setMode(uint8_t node_id, msgs::RunMode mode) override;
             void setLimitQ(uint8_t node_id, double ub, double lb) override;
             void setLimitQd(uint8_t node_id, double qd) override;
             void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) override;
             bool calibrateZeroQ(uint8_t node_id) override;
-            void brake(uint8_t node_id) override;
+            bool torqueOn(uint8_t node_id) override;
+            bool torqueOff(uint8_t node_id) override;
+            bool brakeRelease(uint8_t node_id) override;
+            bool quickStop(uint8_t node_id) override;
             bool reachedTargetQ(uint8_t node_id) override;
 
             double getQ(uint8_t node_id) override;
             double getQd(uint8_t node_id) override;
 
-            void setQd(uint8_t node_id, double qd) override;
-            void setQdd(uint8_t node_id, double qdd) override;
-
-            void torqueOff(uint8_t node_id) override;
+            bool commandProfilePosition(uint8_t node_id,
+                                        double target_q,
+                                        double max_qd,
+                                        double max_qdd) override;
+            bool commandProfileVelocity(uint8_t node_id,
+                                        double target_qd,
+                                        double max_qdd) override;
+            bool commandCyclicPosition(uint8_t node_id,
+                                       double target_q,
+                                       double target_qd) override;
+            bool commandCyclicVelocity(uint8_t node_id,
+                                       double target_qd) override;
+            bool commandCyclicTorque(uint8_t node_id, double target_tau) override;
+            void setMotorConversion(uint8_t node_id,
+                                    double encoder_counts_per_rev,
+                                    double gear_ratio) override;
 
             void seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms = 10);
-            void seedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, msgs::ObIndex index,
-                          msgs::ObSubIndex sub_index, uint32_t data, uint32_t delay_ms = 10);
+            void seedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, uint32_t index,
+                          uint32_t sub_index, uint32_t data, uint32_t delay_ms = 10);
 
             void configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel);
             void configPdo(uint8_t node_id);
@@ -70,14 +83,20 @@ namespace cmvr {
 
 
         private:
-            static constexpr double GearRatio = 101.0; // 电机减速比
             static constexpr double RADTODEG = 180.0 / M_PI;
+            static constexpr double Ti5VelocityUnitScale = 100.0;
+            static constexpr double Ti5AccelerationTimeScale = 1000.0;
+
+            struct MotorConversion {
+                double encoder_counts_per_rev{0.0};
+                double gear_ratio{0.0};
+            };
+
             std::shared_ptr<AbstractCanbus> can_client_{nullptr};
 
             // key node_id
             // std::unordered_map<uint8_t,msgs::RunMode> cur_mode_{};
-            std::unordered_map<uint8_t,uint32_t> last_Qd_{};
-            std::unordered_map<uint8_t,uint32_t> last_Qdd_{};
+            std::unordered_map<uint8_t, MotorConversion> motor_conversions_{};
             std::shared_ptr<device::CanSender<msgs::RobotDetail> > can_sender_{nullptr};
             std::shared_ptr<device::MessageManager<msgs::RobotDetail> > message_manager_{nullptr};
 
@@ -94,11 +113,7 @@ namespace cmvr {
             std::map<uint8_t, motor::Ti5MotorRPDO1 *> rpdo1_commands_{};
             std::map<uint8_t, motor::Ti5MotorRPDO2 *> rpdo2_commands_{};
 
-            void setPPTargetPosBySdo(uint8_t node_id, int32_t pos);
-
-            void setPPTargetPosByPdo(uint8_t node_id, int32_t pos);
-
-            void setCSPTargetPosByPdo(uint8_t node_id, int32_t pos);
+            void writeProfilePositionTargetBySdo(uint8_t node_id, int32_t pos);
 
 
             void configTPDO1(uint8_t node_id);
@@ -107,6 +122,14 @@ namespace cmvr {
             void configRPDO1(uint8_t node_id, bool enable);
             void configRPDO2(uint8_t node_id, bool enable);
 
+            const MotorConversion* conversionForNode(uint8_t node_id) const;
+            double radToCounts(double angle_rad, const MotorConversion& conversion) const;
+            double countsToRad(int32_t counts, const MotorConversion& conversion) const;
+            double radPerSecToVelocityRaw(double velocity_rad_s, const MotorConversion& conversion) const;
+            uint32_t radPerSec2ToAccelerationRaw(double acceleration_rad_s2,
+                                                 const MotorConversion& conversion) const;
+            double velocityRawToRadPerSec(int32_t velocity_raw, const MotorConversion& conversion) const;
+
 
             bool waitUntil(std::function<bool()> condition, int timeout_ms) {
                 auto start = std::chrono::steady_clock::now();

cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_sdo_response.cpp

@@ -3,6 +3,7 @@
 // Created by lgv on 2025/7/24.
 //
 
+#include "cmvr/msgs/cia402.pb.h"
 #include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_sdo_response.h"
 
 using namespace cmvr::device::motor;
@@ -19,17 +20,17 @@ void Ti5MotorSdoResponse::ParseSdoData(const msgs::SdoFrame &sdo_response,
 
 
     switch (sdo_response.index()) {
-        case msgs::CONTROL_WORD_6040:
+        case msgs::CIA402_CONTROL_WORD_6040:
             motor_status->set_ctrl_word(sdo_response.data());
             break;
-        case msgs::STATUS_WORD_6041:
+        case msgs::CIA402_STATUS_WORD_6041:
             motor_status->set_status_word(sdo_response.data());
             break;
-        case msgs::ACTUAL_POSITION_6064:
+        case msgs::CIA402_ACTUAL_POSITION_6064:
             motor_status->set_position(static_cast<int32_t>(sdo_response.data()));
             CMVR_LOG(INFO) << "pos = " << motor_status->position();
             break;
-        case msgs::POSITION_OFFSET_2008:
+        case msgs::CANOPEN_POSITION_OFFSET_2008:
             motor_status->set_position_offset(sdo_response.data());
     }
         //

cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo2.cpp

@@ -19,16 +19,4 @@ void Ti5MotorTPDO2::Parse(const std::uint8_t *bytes, int32_t length, msgs::Robot
 
     motor_status->set_position(bytes[3] << 24 | bytes[2] << 16 | bytes[1] << 8 | bytes[0]);
     motor_status->set_speed(bytes[7] << 24 | bytes[6] << 16 | bytes[5] << 8 | bytes[4]);
-
-
-    double gearRatio = 101.0;
-    double radToDeg = 180.0 / M_PI;
-    auto speed =   (motor_status->speed() * 360.0) / (radToDeg * gearRatio * 100.0);
-
-    auto angle_rad = (motor_status->position() * 360.0) / (gearRatio * 65536.0 * radToDeg);
-
-    // CMVR_LOG(INFO) << " Motor ID " << int(this->node_id_) << " pos = " << angle_rad << " rad speed = " << speed << " rad/s";
-
-
-
 }

cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp

@@ -3,6 +3,7 @@
 // Created by lgv on 2025/8/1.
 //
 
+#include "cmvr/msgs/cia402.pb.h"
 #include "motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h"
 #include "canbus/canopen/register.h"
 #include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_tpdo1.h"
@@ -94,45 +95,153 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) {
     return ErrorCode::OK;
 }
 
+void Ti5MotorCanopenProtocol::setMotorConversion(
+    const uint8_t node_id,
+    const double encoder_counts_per_rev,
+    const double gear_ratio) {
+    motor_conversions_[node_id] = {encoder_counts_per_rev, gear_ratio};
+}
 
-void Ti5MotorCanopenProtocol::seedSdoRequest(uint8_t node_id, CommandSpecifier cs, ObIndex index, ObSubIndex sub_index,
+const Ti5MotorCanopenProtocol::MotorConversion*
+Ti5MotorCanopenProtocol::conversionForNode(const uint8_t node_id) const {
+    const auto it = motor_conversions_.find(node_id);
+    if (it != motor_conversions_.end() &&
+        it->second.encoder_counts_per_rev > 0.0 &&
+        it->second.gear_ratio > 0.0) {
+        return &it->second;
+    }
+    CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] missing conversion config for node "
+                    << static_cast<int>(node_id);
+    return nullptr;
+}
+
+double Ti5MotorCanopenProtocol::radToCounts(
+    const double angle_rad,
+    const MotorConversion& conversion) const {
+    return (angle_rad * RADTODEG) / 360.0 *
+           conversion.gear_ratio * conversion.encoder_counts_per_rev;
+}
+
+double Ti5MotorCanopenProtocol::countsToRad(
+    const int32_t counts,
+    const MotorConversion& conversion) const {
+    return (counts * 360.0) /
+           (conversion.gear_ratio * conversion.encoder_counts_per_rev * RADTODEG);
+}
+
+double Ti5MotorCanopenProtocol::radPerSecToVelocityRaw(
+    const double velocity_rad_s,
+    const MotorConversion& conversion) const {
+    return ((velocity_rad_s * RADTODEG) * conversion.gear_ratio * Ti5VelocityUnitScale) /
+           360.0;
+}
+
+uint32_t Ti5MotorCanopenProtocol::radPerSec2ToAccelerationRaw(
+    const double acceleration_rad_s2,
+    const MotorConversion& conversion) const {
+    const auto raw = ((std::abs(acceleration_rad_s2) * RADTODEG) *
+                      conversion.gear_ratio * Ti5VelocityUnitScale) /
+                     360.0 / Ti5AccelerationTimeScale;
+    return static_cast<uint32_t>(std::abs(raw));
+}
+
+double Ti5MotorCanopenProtocol::velocityRawToRadPerSec(
+    const int32_t velocity_raw,
+    const MotorConversion& conversion) const {
+    return (velocity_raw * 360.0) /
+           (conversion.gear_ratio * Ti5VelocityUnitScale * RADTODEG);
+}
+
+
+void Ti5MotorCanopenProtocol::seedSdoRequest(uint8_t node_id, CommandSpecifier cs, uint32_t index, uint32_t sub_index,
                                              uint32_t data, uint32_t delay_ms) {
     sdo_commands_[node_id]->SetFrameData(cs, index, sub_index, data);
     can_sender_->Update(sdo_commands_[node_id]->ID());
     std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
 }
 
-void Ti5MotorCanopenProtocol::setQ(uint8_t node_id, double angle_rad) {
-    auto cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0;
-
-    switch (getMode(node_id)) {
-        // case RUN_MODE_CYCLIC_SYNC_POSITION:
-        //     setCSPTargetPosByPdo(node_id, static_cast<int32_t>(cmd));
-        //     break;
-        case RUN_MODE_PROFILE_POSITION:
-            // setPPTargetPosByPdo(node_id, static_cast<int32_t>(cmd));
-            setPPTargetPosBySdo(node_id, static_cast<int32_t>(cmd));
-            break;
+bool Ti5MotorCanopenProtocol::commandProfilePosition(uint8_t node_id,
+                                                     double target_q,
+                                                     double max_qd,
+                                                     double max_qdd) {
+    const auto* conversion = conversionForNode(node_id);
+    if (!conversion) {
+        return false;
     }
+    if (max_qd > 0.0) {
+        seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_VELOCITY_6081,
+                       SUB_INDEX_0,
+                       static_cast<uint32_t>(std::abs(radPerSecToVelocityRaw(max_qd, *conversion))),
+                       0);
+    }
+    if (max_qdd > 0.0) {
+        const auto accel = radPerSec2ToAccelerationRaw(max_qdd, *conversion);
+        seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083,
+                       SUB_INDEX_0, accel, 0);
+        seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084,
+                       SUB_INDEX_0, accel, 0);
+    }
+    writeProfilePositionTargetBySdo(node_id, static_cast<int32_t>(radToCounts(target_q, *conversion)));
+    return true;
 }
 
-void Ti5MotorCanopenProtocol::setTarget(uint8_t node_id, double angle_rad, double vel) {
-    auto pos_cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0;
-    auto speed = ((vel * RADTODEG) * GearRatio * 100.0) / 360.0;
+bool Ti5MotorCanopenProtocol::commandProfileVelocity(uint8_t node_id,
+                                                     double target_qd,
+                                                     double max_qdd) {
+    const auto* conversion = conversionForNode(node_id);
+    if (!conversion) {
+        return false;
+    }
+    if (max_qdd > 0.0) {
+        const auto accel = radPerSec2ToAccelerationRaw(max_qdd, *conversion);
+        seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083,
+                       SUB_INDEX_0, accel, 0);
+        seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084,
+                       SUB_INDEX_0, accel, 0);
+    }
+    const auto speed = radPerSecToVelocityRaw(target_qd, *conversion);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_VELOCITY_60FF,
+                   SUB_INDEX_0,
+                   static_cast<uint32_t>(static_cast<int32_t>(std::llround(speed))),
+                   0);
+    return true;
+}
+
+bool Ti5MotorCanopenProtocol::commandCyclicPosition(uint8_t node_id,
+                                                    double target_q,
+                                                    double target_qd) {
+    const auto* conversion = conversionForNode(node_id);
+    if (!conversion) {
+        return false;
+    }
+    auto pos_cmd = radToCounts(target_q, *conversion);
+    auto speed = radPerSecToVelocityRaw(target_qd, *conversion);
     rpdo1_commands_[node_id]->SetTargetPos(pos_cmd);
     rpdo1_commands_[node_id]->SetTargetVel(uint32_t(std::abs(speed)));
     can_sender_->Update(rpdo1_commands_[node_id]->ID());
+    return true;
 }
 
-
-void Ti5MotorCanopenProtocol::setTarget(uint8_t node_id, double vel) {
-    auto speed = ((vel * RADTODEG) * GearRatio * 100.0) / 360.0;
+bool Ti5MotorCanopenProtocol::commandCyclicVelocity(uint8_t node_id,
+                                                    double target_qd) {
+    const auto* conversion = conversionForNode(node_id);
+    if (!conversion) {
+        return false;
+    }
+    auto speed = radPerSecToVelocityRaw(target_qd, *conversion);
     rpdo2_commands_[node_id]->SetTargetVel(int16_t(speed));
     can_sender_->Update(rpdo2_commands_[node_id]->ID());
+    return true;
 }
 
+bool Ti5MotorCanopenProtocol::commandCyclicTorque(uint8_t node_id, double target_tau) {
+    (void)target_tau;
+    CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] cyclic torque command is not implemented, node="
+                    << static_cast<int>(node_id);
+    return false;
+}
 
-void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos) {
+void Ti5MotorCanopenProtocol::writeProfilePositionTargetBySdo(uint8_t node_id, int32_t pos) {
     controlword_t cw = {};
     cw.switch_on = 1;
     cw.enable_voltage = 1;
@@ -141,44 +250,19 @@ void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos)
     cw.change_set_immediately = 1;
 
     // 1. 设置目标位置
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, pos);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A, SUB_INDEX_0, pos);
 
 
     // 2. 设置触发位（bit4 = 1）
     cw.new_set_point = 1;
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
 
 
     // 3. 清除触发位（bit4 = 0），准备下一次触发
     cw.new_set_point = 0;
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
 }
 
-void Ti5MotorCanopenProtocol::setPPTargetPosByPdo(uint8_t node_id, int32_t pos) {
-    // 触发目标位置运动
-    controlword_t cw;
-    cw.value = 0x0F;
-    cw.new_set_point = 1;
-    cw.change_set_immediately = 1;
-
-    rpdo1_commands_[node_id]->SetTargetPos(pos);
-    rpdo1_commands_[node_id]->SetCtrlWord(cw.value);
-    can_sender_->Update(rpdo1_commands_[node_id]->ID());
-
-    std::this_thread::sleep_for(std::chrono::milliseconds(10));
-
-    cw.new_set_point = 0;
-    rpdo1_commands_[node_id]->SetCtrlWord(cw.value);
-    can_sender_->Update(rpdo1_commands_[node_id]->ID());
-}
-
-void Ti5MotorCanopenProtocol::setCSPTargetPosByPdo(uint8_t node_id, int32_t pos) {
-    rpdo1_commands_[node_id]->SetTargetPos(pos);
-    rpdo1_commands_[node_id]->SetCtrlWord(0x0F);
-    can_sender_->Update(rpdo1_commands_[node_id]->ID());
-}
-
-
 void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
     // cur_mode_[node_id] = mode;
 
@@ -188,36 +272,36 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
     controlword_t cw = {};
     cw.quick_stop = 1;
     cw.enable_voltage = 1;
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20);
     // configRPDO1(node_id, false);
     // configRPDO2(node_id, false);
 
     // 1 : 先设置模式
     auto data = static_cast<uint32_t>(mode);
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, OPERATION_MODE_6060, SUB_INDEX_0, data);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CIA402_OPERATION_MODE_6060, SUB_INDEX_0, data);
 
 
     // 3 : 状态机步进 —— Switch On & Enable Operation（0x0F）
     cw.switch_on = 1;
     cw.enable_operation = 1;
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20);
 
 
     switch (mode) {
         case RUN_MODE_PROFILE_POSITION: {
             // 4 : 设置目标位置（为当前位置）
             auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
-            seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos);
+            seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A, SUB_INDEX_0, cur_pos);
 
 
             // 5 : 触发位置运动（new_set_point 翻转）
             cw.new_set_point = 1;
             cw.change_set_immediately = 1;
-            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
+            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
 
             // 6 : 清除 new_set_point（必须，不清除则无法再次触发新目标）
             cw.new_set_point = 0;
-            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
+            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
             break;
         }
 
@@ -225,19 +309,19 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
             // configRPDO1(node_id, true);
             // 设置目标位置为当前位置
             auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
-            seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos);
+            seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A, SUB_INDEX_0, cur_pos);
 
             //3 : 使能   15
             cw.enable_operation = 1;
             cw.switch_on = 1;
-            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
+            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
             break;
         }
 
         case RUN_MODE_PROFILE_VELOCITY: {
             cw.enable_operation = 1;
             cw.switch_on = 1;
-            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
+            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
             break;
         }
 
@@ -245,7 +329,7 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
             // configRPDO2(node_id, true);
             cw.enable_operation = 1;
             cw.switch_on = 1;
-            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
+            seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
             break;
         }
         default:
@@ -262,9 +346,9 @@ void Ti5MotorCanopenProtocol::seedNmtRequest(uint8_t node_id, msgs::NmtCommand c
 
 
 void Ti5MotorCanopenProtocol::configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel) {
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed);
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel);
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, decel);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_VELOCITY_6081, SUB_INDEX_0, speed);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084, SUB_INDEX_0, decel);
 }
 
 
@@ -272,128 +356,128 @@ void Ti5MotorCanopenProtocol::configTPDO1(uint8_t node_id) {
     //TDPO1 配置   状态字 和 控制字
     // 1: 失能 pdo
     uint32_t cob_id = TPDO1_BASE_ID_180 + node_id;
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (1U << 31));
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_MAP_1A00, SUB_INDEX_0, 0);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (1U << 31));
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_0, 0);
 
     // 2: 配置为异步
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_COMM_1800, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
 
     // 3：配置约束时间 unit:0.1ms
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO1_COMM_1800, SUB_INDEX_3, 10);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_3, 10);
 
     // 4 : 配置周期发送时间  unit : ms   0 为 数据改变时发送
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO1_COMM_1800, SUB_INDEX_5, 0);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_5, 0);
 
     // 5 ：映射控制字
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_1,
-                   CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_1,
+                   CIA402_CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16);
 
     //6 : 映射状态字
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_2,
-                   STATUS_WORD_6041 << 16 | SUB_INDEX_0 << 8 | 16);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_2,
+                   CIA402_STATUS_WORD_6041 << 16 | SUB_INDEX_0 << 8 | 16);
 
     //7 : 映射模式
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_3,
-                   MODE_DISPLAY_6061 << 16 | SUB_INDEX_0 << 8 | 8);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_3,
+                   CIA402_MODE_DISPLAY_6061 << 16 | SUB_INDEX_0 << 8 | 8);
 
     //8  映射错误码
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_4,
-                   ERROR_CODE_603F << 16 | SUB_INDEX_0 << 8 | 16);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_4,
+                   CIA402_ERROR_CODE_603F << 16 | SUB_INDEX_0 << 8 | 16);
 
     //9  写入该PDO映射对象总个数
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_MAP_1A00, SUB_INDEX_0, 4);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_0, 4);
 
     //10 使能
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (0U << 31));
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (0U << 31));
 }
 
 
 void Ti5MotorCanopenProtocol::configTPDO2(uint8_t node_id) {
     // 1: 失能 pdo
     uint32_t cob_id = TPDO2_BASE_ID_280 + node_id;
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (1U << 31));
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_MAP_1A01, SUB_INDEX_0, 0);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (1U << 31));
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_0, 0);
 
     // 2: 配置为异步
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_COMM_1801, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
 
     // 3：配置约束时间 unit:0.1ms
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO2_COMM_1801, SUB_INDEX_3, 100);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_3, 100);
 
     // 4 : 配置周期发送时间  unit : ms
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO2_COMM_1801, SUB_INDEX_5, 0);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_5, 0);
 
     // 5 ：映射当前位置
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_MAP_1A01, SUB_INDEX_1,
-                   ACTUAL_POSITION_6064 << 16 | SUB_INDEX_0 << 8 | 32);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_1,
+                   CIA402_ACTUAL_POSITION_6064 << 16 | SUB_INDEX_0 << 8 | 32);
 
     //6 : 映射当前速度
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_MAP_1A01, SUB_INDEX_2,
-                   ACTUAL_SPEED_606C << 16 | SUB_INDEX_0 << 8 | 32);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_2,
+                   CIA402_ACTUAL_VELOCITY_606C << 16 | SUB_INDEX_0 << 8 | 32);
 
     //9  写入该PDO映射对象总个数
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_MAP_1A01, SUB_INDEX_0, 2);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_0, 2);
 
     //10 使能
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (0U << 31));
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (0U << 31));
 }
 
 void Ti5MotorCanopenProtocol::configRPDO1(uint8_t node_id, bool enable) {
     // 1: 失能 pdo
     uint32_t cob_id = RPDO1_BASE_ID_200 + node_id;
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (1U << 31));
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 0);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (1U << 31));
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_0, 0);
 
     // 2: 配置为
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_COMM_1400, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO1_COMM_1400, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
 
     // // 3：配置约束时间 unit:0.1ms
-    // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_3,10);
+    // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,CANOPEN_RPDO1_COMM_1400,SUB_INDEX_3,10);
     //
     // // 4 : 配置周期发送时间  unit : ms   0 为 数据改变时发送
-    // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_5,0);
+    // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,CANOPEN_RPDO1_COMM_1400,SUB_INDEX_5,0);
 
     // 5 ：映射位置
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_1,
-                   TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_1,
+                   CIA402_TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32);
 
     //6 : 映射控制字
 
 
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_2,
-                   PROFILE_SPEED_6081 << 16 | SUB_INDEX_0 << 8 | 32);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_2,
+                   CIA402_PROFILE_VELOCITY_6081 << 16 | SUB_INDEX_0 << 8 | 32);
 
 
     //7  写入该PDO映射对象总个数
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 2);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_0, 2);
 
     //8 使能
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (0U << 31));
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (0U << 31));
 }
 
 
 void Ti5MotorCanopenProtocol::configRPDO2(uint8_t node_id, bool enable) {
     // 1: 失能 pdo
     uint32_t cob_id = RPDO2_BASE_ID_300 + node_id;
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (1U << 31));
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO2_MAP_1601, SUB_INDEX_0, 0);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (1U << 31));
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO2_MAP_1601, SUB_INDEX_0, 0);
 
     if (!enable) return;
 
     // 2: 配置为
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO2_COMM_1401, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO2_COMM_1401, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
 
 
     // 5 ：映射位置
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO2_MAP_1601, SUB_INDEX_1,
-                   TARGET_SPEED_60FF << 16 | SUB_INDEX_0 << 8 | 32);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO2_MAP_1601, SUB_INDEX_1,
+                   CIA402_TARGET_VELOCITY_60FF << 16 | SUB_INDEX_0 << 8 | 32);
 
 
     //7  写入该PDO映射对象总个数
-    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO2_MAP_1601, SUB_INDEX_0, 1);
+    seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO2_MAP_1601, SUB_INDEX_0, 1);
 
     //8 使能
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (0U << 31));
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (0U << 31));
 }
 
 
@@ -405,36 +489,49 @@ void Ti5MotorCanopenProtocol::configPdo(uint8_t node_id) {
 }
 
 void Ti5MotorCanopenProtocol::setLimitQdd(uint8_t node_id, double u_qdd, double l_qdd) {
-    auto accel = ((u_qdd * RADTODEG) * GearRatio * 100.0) / 360.0 / 1000.0;
-    auto decel = ((l_qdd * RADTODEG) * GearRatio * 100.0) / 360.0 / 1000.0;
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, std::abs(accel));
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, std::abs(decel));
+    const auto* conversion = conversionForNode(node_id);
+    if (!conversion) {
+        return;
+    }
+    auto accel = ((u_qdd * RADTODEG) * conversion->gear_ratio * Ti5VelocityUnitScale) /
+                 360.0 / Ti5AccelerationTimeScale;
+    auto decel = ((l_qdd * RADTODEG) * conversion->gear_ratio * Ti5VelocityUnitScale) /
+                 360.0 / Ti5AccelerationTimeScale;
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083, SUB_INDEX_0, std::abs(accel));
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084, SUB_INDEX_0, std::abs(decel));
 }
 
 void Ti5MotorCanopenProtocol::setLimitQd(uint8_t node_id, double qd) {
-    auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0;
-    // seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, MAX_SPEED_607F, SUB_INDEX_0, speed);
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed);
+    const auto* conversion = conversionForNode(node_id);
+    if (!conversion) {
+        return;
+    }
+    auto speed = radPerSecToVelocityRaw(qd, *conversion);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_MAX_PROFILE_VELOCITY_607F, SUB_INDEX_0, speed);
 }
 
 void Ti5MotorCanopenProtocol::setLimitQ(uint8_t node_id, double ub, double lb) {
-    ub = (ub * RADTODEG) / 360.0 * GearRatio * 65536.0;
-    lb = (lb * RADTODEG) / 360.0 * GearRatio * 65536.0;
+    const auto* conversion = conversionForNode(node_id);
+    if (!conversion) {
+        return;
+    }
+    ub = radToCounts(ub, *conversion);
+    lb = radToCounts(lb, *conversion);
 
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_1, lb);
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_2, ub);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_1, lb);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_2, ub);
 }
 
 
 bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) {
     // 0: 设置控制字为 0x06，确保停机状态
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x06, 1000);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x06, 1000);
 
     // 1: 清除偏置值 0x2008 ← 0
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, 0);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, 0);
 
     // 2: 等待确认清除成功
-    seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0);
+    seedSdoRequest(node_id, CS_READ_REQUEST, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, 0);
     if (!waitUntil([&]() {
         return GetRobotDetail()->motors().at(node_id).position_offset() == 0;
     }, 1000)) {
@@ -443,18 +540,18 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) {
     }
 
     // 3: 读取当前位置 0x6064
-    seedSdoRequest(node_id, CS_READ_REQUEST, ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20);
+    seedSdoRequest(node_id, CS_READ_REQUEST, CIA402_ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20);
     auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
 
     // 4: 将当前位置写入偏置寄存器
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, cur_pos);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, cur_pos);
 
     // 5: 保存参数到永久区（0x2000 ← 1）
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, USER_SAVE_PARA_2000, SUB_INDEX_0, 1, 100);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_USER_SAVE_PARA_2000, SUB_INDEX_0, 1, 100);
 
 
     // 6: 确认写入成功
-    seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0, 20);
+    seedSdoRequest(node_id, CS_READ_REQUEST, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, 0, 20);
     if (!waitUntil([&]() {
         return GetRobotDetail()->motors().at(node_id).position_offset() == cur_pos;
     }, 500)) {
@@ -465,16 +562,28 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) {
     return true;
 }
 
-void Ti5MotorCanopenProtocol::brake(uint8_t node_id) {
+bool Ti5MotorCanopenProtocol::torqueOn(uint8_t node_id) {
+    setMode(node_id, msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
+    return true;
+}
+
+bool Ti5MotorCanopenProtocol::brakeRelease(uint8_t node_id) {
+    (void)node_id;
+    CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] brakeRelease is not implemented";
+    return false;
+}
+
+bool Ti5MotorCanopenProtocol::quickStop(uint8_t node_id) {
     // // 开机未使能电机时调用
-    // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
+    // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
     // 6 抱闸   0 ： 立即停机 自由
-    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, QUICK_STOP_DECEL_6085, SUB_INDEX_0, 0XFFFFFFF0);
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, QUICK_STOP_OPTION_605A, SUB_INDEX_0, 6);
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 100);
+    seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_QUICK_STOP_DECELERATION_6085, SUB_INDEX_0, 0XFFFFFFF0);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_QUICK_STOP_OPTION_605A, SUB_INDEX_0, 6);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 100);
 
     // 必须要发送 0xf 才能按照6085中设定的减速度减速
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
+    return true;
 }
 
 bool Ti5MotorCanopenProtocol::reachedTargetQ(uint8_t node_id) {
@@ -483,62 +592,37 @@ bool Ti5MotorCanopenProtocol::reachedTargetQ(uint8_t node_id) {
     return st.target_reached == 1;
 }
 
-void Ti5MotorCanopenProtocol::setQd(uint8_t node_id, double qd) {
-    auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0;
-    switch (getMode(node_id)) {
-        case msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
-        case msgs::RUN_MODE_PROFILE_POSITION: {
-            auto it = last_Qd_.find(node_id);
-            if (it == last_Qd_.end() || it->second != speed) {
-                seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, uint32_t(std::abs(speed)),
-                               0);
-                last_Qd_[node_id] = speed;
-            }
-            break;
-        }
-        case msgs::RUN_MODE_PROFILE_VELOCITY:
-        case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY: {
-            // 在速度模式下，直接设置目标速度
-            seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_SPEED_60FF, SUB_INDEX_0, uint32_t(speed), 0);
-            break;
-        }
-        default:
-            break;
-    }
-}
-
-void Ti5MotorCanopenProtocol::setQdd(uint8_t node_id, double qdd) {
-    uint32_t accel = ((std::abs(qdd) * RADTODEG) * GearRatio * 100.0 * 65536.0) / (360.0 * 1000.0);
-    auto it = last_Qdd_.find(node_id);
-    if (it == last_Qdd_.end() || it->second != accel) {
-        seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel);
-        seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, accel);
-        last_Qdd_[node_id] = accel;
-    }
-}
-
-void Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id) {
+bool Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id) {
     // 0 ： 立即停机 自由
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, QUICK_STOP_OPTION_605A, SUB_INDEX_0, 0);
-    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 20);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_QUICK_STOP_OPTION_605A, SUB_INDEX_0, 0);
+    seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 20);
     // 必须要发送 0xf 才能按照6085中设定的减速度减速
-    // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
+    // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
 
     // 停机之后，要重新使能？
     // cur_mode_[node_id] = msgs::RUN_MODE_UNSPECIFIED;
+    return true;
 }
 
 
 double Ti5MotorCanopenProtocol::getQ(uint8_t node_id) {
+    const auto* conversion = conversionForNode(node_id);
+    if (!conversion) {
+        return 0.0;
+    }
     auto data_ptr = std::make_unique<msgs::RobotDetail>();
     message_manager_->GetSensorData(data_ptr.get());
     auto cnt = data_ptr->motors().at(node_id).position();
-    return (cnt * 360.0) / (GearRatio * 65536.0 * RADTODEG);
+    return countsToRad(cnt, *conversion);
 }
 
 double Ti5MotorCanopenProtocol::getQd(uint8_t node_id) {
+    const auto* conversion = conversionForNode(node_id);
+    if (!conversion) {
+        return 0.0;
+    }
     auto data_ptr = std::make_unique<msgs::RobotDetail>();
     message_manager_->GetSensorData(data_ptr.get());
     auto cnt = data_ptr->motors().at(node_id).speed();
-    return (cnt * 360.0) / (GearRatio * 100.0 * RADTODEG);
+    return velocityRawToRadPerSec(cnt, *conversion);
 }

cmvr-es/devices/motor/manager/CMakeLists.txt

@@ -12,6 +12,7 @@ target_link_libraries(motor_manager
         PRIVATE
         cmvr_es::device::ti5_canopen_motor_driver
         cmvr_es::device::mujoco_motor_driver
+        cmvr_es::device::ethercat_motor_driver
         cmvr_es::ik_solver
         glog
 )

cmvr-es/devices/motor/manager/src/motor_manager.cpp

@@ -1,21 +1,27 @@
-#include "motor/manager/include/motor_manager.h"
+#include "devices/motor/manager/include/motor_manager.h"
 
+#include <chrono>
 #include <cmath>
 #include <cstddef>
 #include <cstdint>
+#include <thread>
 #include <unordered_map>
 #include <utility>
 
 #include "algorithms/kinematics/ik_solver/common/include/urdf_parser.h"
 #include "common/base/logging/logger.h"
 #include "common/config/config_files.h"
-#include "../../bus_runtime/abstract_motor_bus_runtime.h"
-#include "motor/bus_runtime/can/include/can_motor_bus_runtime.h"
-#include "motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
-#include "motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h"
-#include "motor/drivers/mujoco/include/mujoco_motor.h"
-#include "motor/drivers/ti5_canopen/include/ti5_motor.h"
-#include "motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h"
+#include "devices/motor/bus_runtime/abstract_motor_bus_runtime.h"
+#include "devices/motor/bus_runtime/can/include/can_motor_bus_runtime.h"
+#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
+#include "devices/motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h"
+#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h"
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h"
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h"
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h"
+#include "devices/motor/drivers/mujoco/include/mujoco_motor.h"
+#include "devices/motor/drivers/ti5_canopen/include/ti5_motor.h"
+#include "devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h"
 
 namespace cmvr::device {
 
@@ -91,11 +97,17 @@ bool MotorManager::init()
             all_ok = false;
             continue;
         }
-        if (!bus_runtime->start()) {
+
+        const bool start_before_motor_init =
+            motor_group_cfg.bus_type() == config::MOTOR_BUS_ETHERCAT;
+        if (start_before_motor_init && !bus_runtime->start()) {
             bus_runtime->stop();
             all_ok = false;
             continue;
         }
+        if (start_before_motor_init) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+        }
 
         auto motors = createMotors_(motor_group_cfg, selected_motor_cfgs, bus_runtime);
         if (motors.empty()) {
@@ -116,6 +128,11 @@ bool MotorManager::init()
             all_ok = false;
             continue;
         }
+        if (!start_before_motor_init && !bus_runtime->start()) {
+            bus_runtime->stop();
+            all_ok = false;
+            continue;
+        }
 
         bus_runtimes_.push_back(std::move(bus_runtime));
     }
@@ -398,8 +415,19 @@ std::shared_ptr<AbstractMotorBusRuntime> MotorManager::createBusRuntime_(
             return std::make_shared<CanMotorBusRuntime>();
         case config::MOTOR_BUS_MUJOCO:
             return std::make_shared<MujocoMotorBusRuntime>();
-        case config::MOTOR_BUS_ETHERCAT:
-            return std::make_shared<EthercatMotorBusRuntime>();
+        case config::MOTOR_BUS_ETHERCAT: {
+            auto runtime = std::make_shared<EthercatMotorBusRuntime>();
+            if (group_cfg.vendor() == config::MOTOR_VENDOR_EYOU &&
+                group_cfg.protocol() == config::MOTOR_PROTOCOL_ETHERCAT_CIA402) {
+                runtime->setPdoMapping(createEyouCia402PdoMapping());
+                return runtime;
+            }
+            CMVR_LOG(ERROR) << "[MotorManager] unsupported EtherCAT motor: vendor="
+                            << config::MotorVendor_Name(group_cfg.vendor())
+                            << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol())
+                            << ", group=" << group_cfg.id();
+            return nullptr;
+        }
         default:
             CMVR_LOG(ERROR) << "[MotorManager] unsupported motor bus type: "
                             << config::MotorBusType_Name(group_cfg.bus_type())
@@ -534,6 +562,14 @@ std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createEthercatMotors_(
         CMVR_LOG(ERROR) << "[MotorManager] missing EtherCAT config: " << group_cfg.id();
         return {};
     }
+    if (group_cfg.vendor() != config::MOTOR_VENDOR_EYOU ||
+        group_cfg.protocol() != config::MOTOR_PROTOCOL_ETHERCAT_CIA402) {
+        CMVR_LOG(ERROR) << "[MotorManager] unsupported EtherCAT motor: vendor="
+                        << config::MotorVendor_Name(group_cfg.vendor())
+                        << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol())
+                        << ", group=" << group_cfg.id();
+        return {};
+    }
     for (const auto& motor_cfg : motor_cfgs) {
         if (!ethercat_bus_runtime->slaveForMotor(motor_cfg.id())) {
             CMVR_LOG(ERROR) << "[MotorManager] missing EtherCAT slave config for motor id "
@@ -542,11 +578,22 @@ std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createEthercatMotors_(
         }
     }
 
-    CMVR_LOG(ERROR) << "[MotorManager] EtherCAT motor creation is not implemented: vendor="
-                    << config::MotorVendor_Name(group_cfg.vendor())
-                    << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol())
-                    << ", group=" << group_cfg.id();
-    return {};
+    auto protocol = std::make_shared<Cia402Protocol>(
+        ethercat_bus_runtime, group_cfg.ethercat().cia402());
+
+    std::vector<std::shared_ptr<AbstractMotor>> motors;
+    motors.reserve(motor_cfgs.size());
+    for (const auto& cfg : motor_cfgs) {
+        auto motor = std::make_shared<EyouMotor>(
+            cfg, protocol, std::make_unique<EyouMotorAdapter>(ethercat_bus_runtime));
+        if (!motor->init()) {
+            CMVR_LOG(ERROR) << "[MotorManager] failed to init EYOU EtherCAT motor: "
+                            << cfg.joint_name();
+            return {};
+        }
+        motors.push_back(std::move(motor));
+    }
+    return motors;
 }
 
 } // namespace cmvr::device

cmvr-es/devices/motor/motor_protocol_interface.h

@@ -15,7 +15,8 @@ namespace cmvr {
         public:
             enum class CommProto : uint8_t {
                 CANOPEN = 1,
-                CUSTOM = 2
+                ETHERCAT = 2,
+                CUSTOM = 3
             };
             virtual ~MotorProtocolInterface() = default;
 
@@ -26,9 +27,6 @@ namespace cmvr {
              */
             virtual bool initNode(uint8_t node_id) = 0;
 
-            virtual void setQ(uint8_t node_id, double angle_rad) = 0;
-            virtual void setTarget(uint8_t node_id, double angle_rad,double vel) = 0;
-            virtual void setTarget(uint8_t node_id,double vel) = 0;
             virtual void setMode(uint8_t node_id,msgs::RunMode mode ) = 0;
             virtual msgs::RunMode getMode(uint8_t node_id) = 0;
             virtual void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) = 0;
@@ -36,12 +34,35 @@ namespace cmvr {
             virtual void setLimitQ(uint8_t node_id, double ub, double lb) = 0;
             virtual bool calibrateZeroQ(uint8_t node_id) = 0;
             virtual bool reachedTargetQ(uint8_t node_id) = 0;
-            virtual void setQd(uint8_t node_id, double qd) = 0;
-            virtual void setQdd(uint8_t node_id,double qdd) = 0;
-            // virtual void setVelocity(uint8_t node_id, double velocity) = 0;
-            // virtual void clearError(uint8_t node_id) = 0;
-            virtual void brake(uint8_t node_id) = 0;
-            virtual void torqueOff(uint8_t node_id) = 0;
+            // target_q: rad, max_qd: rad/s, max_qdd: rad/s^2.
+            // Profile Position 写入目标位置和轮廓速度/加速度，并触发一次新目标。
+            virtual bool commandProfilePosition(uint8_t node_id,
+                                                double target_q,
+                                                double max_qd,
+                                                double max_qdd) = 0;
+            // target_qd: rad/s, max_qdd: rad/s^2.
+            // Profile Velocity 写入目标速度和轮廓加速度。
+            virtual bool commandProfileVelocity(uint8_t node_id,
+                                                double target_qd,
+                                                double max_qdd) = 0;
+            // target_q: rad, target_qd: rad/s.
+            // Cyclic Position 周期写入目标位置和目标速度。
+            virtual bool commandCyclicPosition(uint8_t node_id,
+                                                double target_q,
+                                               double target_qd) = 0;
+            // target_qd: rad/s.
+            // Cyclic Velocity 周期写入目标速度。
+            virtual bool commandCyclicVelocity(uint8_t node_id,
+                                               double target_qd) = 0;
+            // target_tau: N*m.
+            virtual bool commandCyclicTorque(uint8_t node_id, double target_tau) = 0;
+            virtual void setMotorConversion(uint8_t node_id,
+                                            double encoder_counts_per_rev,
+                                            double gear_ratio) = 0;
+            virtual bool torqueOn(uint8_t node_id) = 0;
+            virtual bool torqueOff(uint8_t node_id) = 0;
+            virtual bool brakeRelease(uint8_t node_id) = 0;
+            virtual bool quickStop(uint8_t node_id) = 0;
 
             virtual double getQ(uint8_t node_id) = 0;
             virtual double getQd(uint8_t node_id) = 0;

cmvr-es/manager/device_manager/src/device_manager.cpp

@@ -26,6 +26,9 @@ using namespace cmvr::device;
 
 namespace {
 
+using GroupJointSelection = std::unordered_map<std::string, std::unordered_set<std::string>>;
+using MotorJointSelections = std::unordered_map<std::string, GroupJointSelection>;
+
 void logSection(const char* title)
 {
     CMVR_LOG(INFO) << "---------------- " << title << " ----------------";
@@ -71,6 +74,32 @@ bool motorGroupHasJoint(const cmvr::config::MotorGroupConfig& motor_group,
     return false;
 }
 
+void addAllMotorJoints(const std::string& motor_system_id,
+                       const cmvr::config::MotorRootConfig& root_cfg,
+                       MotorJointSelections& selections)
+{
+    auto& group_selection = selections[motor_system_id];
+    for (const auto& motor_group : root_cfg.motor().motor_groups()) {
+        if (motor_group.id().empty()) {
+            continue;
+        }
+
+        auto& selected_joints = group_selection[motor_group.id()];
+        for (const auto& motor : motor_group.motors().motors()) {
+            if (!motor.joint_name().empty()) {
+                selected_joints.insert(motor.joint_name());
+            }
+        }
+        if (selected_joints.empty()) {
+            group_selection.erase(motor_group.id());
+        }
+    }
+
+    if (group_selection.empty()) {
+        selections.erase(motor_system_id);
+    }
+}
+
 } // namespace
 
 template std::shared_ptr<AbstractAGV> DeviceManager::getDevice(const std::string& device_id);
@@ -256,8 +285,7 @@ void DeviceManager::log_device_plan_() const
 
 void DeviceManager::pre_scan_robot_arm_dependencies_() const
 {
-    using GroupJointSelection = std::unordered_map<std::string, std::unordered_set<std::string>>;
-    std::unordered_map<std::string, GroupJointSelection> selections;
+    MotorJointSelections selections;
     std::unordered_map<std::string, config::MotorRootConfig> motor_roots;
 
     for (const auto& entry : cfg_.devices()) {
@@ -387,6 +415,15 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
         }
     }
 
+    if (selections.empty() && cfg_.init_all_motors_when_no_active_joints()) {
+        CMVR_LOG(INFO) << "[DeviceManager]: No active motor joints from RobotArm; "
+                       << "initialize all configured motors because "
+                       << "init_all_motors_when_no_active_joints=true";
+        for (const auto& [motor_system_id, root_cfg] : motor_roots) {
+            addAllMotorJoints(motor_system_id, root_cfg, selections);
+        }
+    }
+
     MotorManager::clearActiveJoints();
     for (auto& [motor_system_id, group_selection] : selections) {
         MotorManager::setActiveJoints(motor_system_id, std::move(group_selection));

dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf

@@ -0,0 +1,3 @@
+MASTER0_DEVICE="a0:ad:9f:c4:c2:2c"
+DEVICE_MODULES="generic"
+UPDOWN_INTERFACES="eno1"

dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf.bak.20260702_140412

@@ -0,0 +1,113 @@
+#------------------------------------------------------------------------------
+#
+#  EtherCAT master configuration file for use with ethercatctl.
+#
+#  vim: spelllang=en spell tw=78
+#
+#------------------------------------------------------------------------------
+
+#
+# Main Ethernet devices.
+#
+# The MASTER<X>_DEVICE variable specifies the Ethernet device for a master
+# with index 'X'.
+#
+# Specify the MAC address (hexadecimal with colons) of the Ethernet device to
+# use. Example: "00:00:08:44:ab:66"
+#
+# Alternatively, a network interface name can be specified. The interface
+# name will be resolved to a MAC address using the 'ip' command.
+# Example: "eth0"
+#
+# The broadcast address "ff:ff:ff:ff:ff:ff" has a special meaning: It tells
+# the master to accept the first device offered by any Ethernet driver.
+#
+# The MASTER<X>_DEVICE variables also determine, how many masters will be
+# created: A non-empty variable MASTER0_DEVICE will create one master, adding
+# a non-empty variable MASTER1_DEVICE will create a second master, and so on.
+#
+# Examples:
+# MASTER0_DEVICE="00:00:08:44:ab:66"
+# MASTER0_DEVICE="eth0"
+#
+MASTER0_DEVICE=""
+#MASTER1_DEVICE=""
+
+#
+# Backup Ethernet devices
+#
+# The MASTER<X>_BACKUP variables specify the devices used for redundancy. They
+# behaves nearly the same as the MASTER<X>_DEVICE variable, except that it
+# does not interpret the ff:ff:ff:ff:ff:ff address.
+#
+#MASTER0_BACKUP=""
+
+#
+# Ethernet driver modules to use for EtherCAT operation.
+#
+# Specify a non-empty list of Ethernet drivers, that shall be used for
+# EtherCAT operation.
+#
+# Except for the generic Ethernet driver module, the init script will try to
+# unload the usual Ethernet driver modules in the list and replace them with
+# the EtherCAT-capable ones. If a certain (EtherCAT-capable) driver is not
+# found, a warning will appear.
+#
+# Possible values: 8139too, e100, e1000, e1000e, r8169, generic, ccat, igb,
+#                  igc, genet, dwmac-intel, stmmac-pci.
+# Separate multiple drivers with spaces.
+# A list of all matching kernel versions can be found here:
+# https://docs.etherlab.org/ethercat/1.6/doxygen/devicedrivers.html
+#
+# Note: The e100, e1000, e1000e, r8169, ccat, igb and igc drivers are not
+# built by default. Enable them with the --enable-<driver> configure switches.
+#
+DEVICE_MODULES=""
+
+# If you have any issues about network interfaces not being configured
+# properly, systemd may need some additional infos about your setup.
+# Have a look at the service file, you'll find some details there.
+#
+
+#
+# List of interfaces to bring up and down automatically.
+#
+# Specify a space-separated list of interface names (such as eth0 or
+# enp0s1) that shall be brought up on `ethercatctl start` and down on
+# `ethercatctl stop`.
+#
+# When using the generic driver, the corresponding Ethernet device has to be
+# activated before the master is started, otherwise all frames will time out.
+# This the perfect use-case for `UPDOWN_INTERFACES`.
+#
+UPDOWN_INTERFACES=""
+
+#
+# Default SII caching method.
+#
+# Set the start-up caching method for all masters. The integer value
+# determines, which fields are used to look up a cached SII page. It is a
+# bit-field consisting of the following flags. A value of zero disables SII
+# caching (default). A typical value is 7 (use vendor ID, product code and
+# revision number for lookup).
+#
+#   -  1: Vendor ID (always used)
+#   -  2: Product code (always used)
+#   -  4: Revision number
+#   -  8: Serial number
+#   - 16: Alias address
+#
+# Please keep in mind that in case serial number or alias address is enabled,
+# only slaves with a non-zero serial number or alias benefit from caching.
+#
+SII_CACHING=0
+
+#
+# Flags for loading kernel modules.
+#
+# This can usually be left empty. Adjust this variable, if you have problems
+# with module loading.
+#
+#MODPROBE_FLAGS="-b"
+
+#------------------------------------------------------------------------------

dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf.bak.20260702_155334

@@ -0,0 +1,3 @@
+MASTER0_DEVICE="a0:ad:9f:c4:c2:2c"
+DEVICE_MODULES="generic"
+UPDOWN_INTERFACES="eno1"

dependency/x86/third_party/ethercat/v1.7.0/etc/init.d/ethercat

@@ -0,0 +1,123 @@
+#!/bin/sh
+
+#------------------------------------------------------------------------------
+#
+#  Init script for EtherCAT
+#
+#  Copyright (C) 2006-2021  Florian Pose, Ingenieurgemeinschaft IgH
+#
+#  This file is part of the IgH EtherCAT Master.
+#
+#  The IgH EtherCAT Master is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License version 2, as
+#  published by the Free Software Foundation.
+#
+#  The IgH EtherCAT Master is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General
+#  Public License for more details.
+#
+#  You should have received a copy of the GNU General Public License along
+#  with the IgH EtherCAT Master; if not, write to the Free Software
+#  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+#
+#
+#  vim: expandtab
+#
+#------------------------------------------------------------------------------
+
+### BEGIN INIT INFO
+# Provides:          ethercat
+# Required-Start:    $local_fs $syslog $network
+# Should-Start:      $time ntp
+# Required-Stop:     $local_fs $syslog $network
+# Should-Stop:       $time ntp
+# Default-Start:     3 5
+# Default-Stop:      0 1 2 6
+# Short-Description: EtherCAT master
+# Description:       EtherCAT master 1.7.0
+### END INIT INFO
+
+#------------------------------------------------------------------------------
+
+ETHERCATCTL="/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/sbin/ethercatctl -c /home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/etc/sysconfig/ethercat"
+
+#------------------------------------------------------------------------------
+
+exit_success() {
+    if [ -r /etc/rc.status ]; then
+        rc_reset
+        rc_status -v
+        rc_exit
+    else
+        echo " done"
+        exit 0
+    fi
+}
+
+#------------------------------------------------------------------------------
+
+exit_fail() {
+    if [ -r /etc/rc.status ]; then
+        rc_failed
+        rc_status -v
+        rc_exit
+    else
+        echo " failed"
+        exit 1
+    fi
+}
+
+#------------------------------------------------------------------------------
+
+if [ -r /etc/rc.status ]; then
+    . /etc/rc.status
+    rc_reset
+fi
+
+case "${1}" in
+
+start)
+    echo -n "Starting EtherCAT master 1.7.0 "
+
+    if $ETHERCATCTL start; then
+        exit_success
+    else
+        exit_fail
+    fi
+    ;;
+
+stop)
+    echo -n "Shutting down EtherCAT master 1.7.0 "
+
+    if $ETHERCATCTL stop; then
+        exit_success
+    else
+        exit_fail
+    fi
+    ;;
+
+restart)
+    $0 stop || exit 1
+    sleep 1
+    $0 start
+    ;;
+
+status)
+    $ETHERCATCTL status
+    exit $?
+    ;;
+
+*)
+    echo "USAGE: $0 {start|stop|restart|status}"
+    ;;
+
+esac
+
+if [ -r /etc/rc.status ]; then
+    rc_exit
+else
+    exit 1
+fi
+
+#------------------------------------------------------------------------------

dependency/x86/third_party/ethercat/v1.7.0/etc/sysconfig/ethercat

@@ -0,0 +1,113 @@
+#------------------------------------------------------------------------------
+#
+#  EtherCAT master configuration file for use with init.d.
+#
+#  vim: spelllang=en spell tw=78
+#
+#------------------------------------------------------------------------------
+
+#
+# Main Ethernet devices.
+#
+# The MASTER<X>_DEVICE variable specifies the Ethernet device for a master
+# with index 'X'.
+#
+# Specify the MAC address (hexadecimal with colons) of the Ethernet device to
+# use. Example: "00:00:08:44:ab:66"
+#
+# Alternatively, a network interface name can be specified. The interface
+# name will be resolved to a MAC address using the 'ip' command.
+# Example: "eth0"
+#
+# The broadcast address "ff:ff:ff:ff:ff:ff" has a special meaning: It tells
+# the master to accept the first device offered by any Ethernet driver.
+#
+# The MASTER<X>_DEVICE variables also determine, how many masters will be
+# created: A non-empty variable MASTER0_DEVICE will create one master, adding
+# a non-empty variable MASTER1_DEVICE will create a second master, and so on.
+#
+# Examples:
+# MASTER0_DEVICE="00:00:08:44:ab:66"
+# MASTER0_DEVICE="eth0"
+#
+MASTER0_DEVICE=""
+#MASTER1_DEVICE=""
+
+#
+# Backup Ethernet devices
+#
+# The MASTER<X>_BACKUP variables specify the devices used for redundancy. They
+# behaves nearly the same as the MASTER<X>_DEVICE variable, except that it
+# does not interpret the ff:ff:ff:ff:ff:ff address.
+#
+#MASTER0_BACKUP=""
+
+#
+# Ethernet driver modules to use for EtherCAT operation.
+#
+# Specify a non-empty list of Ethernet drivers, that shall be used for
+# EtherCAT operation.
+#
+# Except for the generic Ethernet driver module, the init script will try to
+# unload the usual Ethernet driver modules in the list and replace them with
+# the EtherCAT-capable ones. If a certain (EtherCAT-capable) driver is not
+# found, a warning will appear.
+#
+# Possible values: 8139too, e100, e1000, e1000e, r8169, generic, ccat, igb,
+#                  igc, genet, dwmac-intel, stmmac-pci.
+# Separate multiple drivers with spaces.
+# A list of all matching kernel versions can be found here:
+# https://docs.etherlab.org/ethercat/1.6/doxygen/devicedrivers.html
+#
+# Note: The e100, e1000, e1000e, r8169, ccat, igb and igc drivers are not
+# built by default. Enable them with the --enable-<driver> configure switches.
+#
+DEVICE_MODULES=""
+
+# If you have any issues about network interfaces not being configured
+# properly, systemd may need some additional infos about your setup.
+# Have a look at the service file, you'll find some details there.
+#
+
+#
+# List of interfaces to bring up and down automatically.
+#
+# Specify a space-separated list of interface names (such as eth0 or
+# enp0s1) that shall be brought up on `ethercatctl start` and down on
+# `ethercatctl stop`.
+#
+# When using the generic driver, the corresponding Ethernet device has to be
+# activated before the master is started, otherwise all frames will time out.
+# This the perfect use-case for `UPDOWN_INTERFACES`.
+#
+UPDOWN_INTERFACES=""
+
+#
+# Default SII caching method.
+#
+# Set the start-up caching method for all masters. The integer value
+# determines, which fields are used to look up a cached SII page. It is a
+# bit-field consisting of the following flags. A value of zero disables SII
+# caching (default). A typical value is 7 (use vendor ID, product code and
+# revision number for lookup).
+#
+#   -  1: Vendor ID (always used)
+#   -  2: Product code (always used)
+#   -  4: Revision number
+#   -  8: Serial number
+#   - 16: Alias address
+#
+# Please keep in mind that in case serial number or alias address is enabled,
+# only slaves with a non-zero serial number or alias benefit from caching.
+#
+SII_CACHING=0
+
+#
+# Flags for loading kernel modules.
+#
+# This can usually be left empty. Adjust this variable, if you have problems
+# with module loading.
+#
+#MODPROBE_FLAGS="-b"
+
+#------------------------------------------------------------------------------

dependency/x86/third_party/ethercat/v1.7.0/include/ectty.h

@@ -0,0 +1,106 @@
+/*****************************************************************************
+ *
+ *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH
+ *
+ *  This file is part of the IgH EtherCAT master userspace library.
+ *
+ *  The IgH EtherCAT master userspace library is free software; you can
+ *  redistribute it and/or modify it under the terms of the GNU Lesser General
+ *  Public License as published by the Free Software Foundation; version 2.1
+ *  of the License.
+ *
+ *  The IgH EtherCAT master userspace library is distributed in the hope that
+ *  it will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ *  warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public License
+ *  along with the IgH EtherCAT master userspace library. If not, see
+ *  <http://www.gnu.org/licenses/>.
+ *
+ ****************************************************************************/
+
+/** \file
+ *
+ * EtherCAT virtual TTY interface.
+ *
+ * \defgroup TTYInterface EtherCAT Virtual TTY Interface
+ *
+ * @{
+ */
+
+/****************************************************************************/
+
+#ifndef __ECTTY_H__
+#define __ECTTY_H__
+
+#include <linux/termios.h>
+
+/*****************************************************************************
+ * Data types
+ ****************************************************************************/
+
+struct ec_tty;
+typedef struct ec_tty ec_tty_t; /**< \see ec_tty */
+
+/** Operations on the virtual TTY interface.
+ */
+typedef struct {
+    int (*cflag_changed)(void *, tcflag_t); /**< Called when the serial
+                                              * settings shall be changed. The
+                                              * \a cflag argument contains the
+                                              * new settings. */
+} ec_tty_operations_t;
+
+/*****************************************************************************
+ * Global functions
+ ****************************************************************************/
+
+/** Create a virtual TTY interface.
+ *
+ * \param ops Set of callbacks.
+ * \param cb_data Arbitrary data, that is passed to any callback.
+ *
+ * \return Pointer to the interface object, otherwise an ERR_PTR value.
+ */
+ec_tty_t *ectty_create(
+        const ec_tty_operations_t *ops,
+        void *cb_data
+        );
+
+/*****************************************************************************
+ * TTY interface methods
+ ****************************************************************************/
+
+/** Releases a virtual TTY interface.
+ */
+void ectty_free(
+        ec_tty_t *tty /**< TTY interface. */
+        );
+
+/** Reads data to send from the TTY interface.
+ *
+ * If there are data to send, they are copied into the \a buffer. At maximum,
+ * \a size bytes are copied. The actual number of bytes copied is returned.
+ *
+ * \return Number of bytes copied.
+ */
+unsigned int ectty_tx_data(
+        ec_tty_t *tty, /**< TTY interface. */
+        uint8_t *buffer, /**< Buffer for data to transmit. */
+        size_t size /**< Available space in \a buffer. */
+        );
+
+/** Pushes received data to the TTY interface.
+ */
+void ectty_rx_data(
+        ec_tty_t *tty, /**< TTY interface. */
+        const uint8_t *buffer, /**< Buffer with received data. */
+        size_t size /**< Number of bytes in \a buffer. */
+        );
+
+/****************************************************************************/
+
+/** @} */
+
+#endif

dependency/x86/third_party/ethercat/v1.7.0/lib/cmake/ethercat/ethercat-config.cmake

@@ -0,0 +1,43 @@
+#----------------------------------------------------------------------------
+#
+#  Copyright (C) 2021 Bjarne von Horn, Ingenieurgemeinschaft IgH
+#
+#  This file is part of the IgH EtherCAT Master.
+#
+#  The IgH EtherCAT Master is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License version 2, as
+#  published by the Free Software Foundation.
+#
+#  The IgH EtherCAT Master is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General
+#  Public License for more details.
+#
+#  You should have received a copy of the GNU General Public License along
+#  with the IgH EtherCAT Master; if not, write to the Free Software
+#  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+#
+#  vim: tw=78
+#
+#----------------------------------------------------------------------------
+
+
+find_library(EtherCAT_LIBRARY
+    NAMES ethercat
+    PATHS /home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/lib
+)
+
+find_path(EtherCAT_INCLUDE_DIR
+    NAMES ecrt.h
+    PATHS /home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/include
+)
+
+mark_as_advanced(EtherCAT_LIBRARY EtherCAT_INCLUDE_DIR)
+
+if(NOT TARGET EtherLab::ethercat)
+    add_library(EtherLab::ethercat SHARED IMPORTED)
+    set_target_properties(EtherLab::ethercat PROPERTIES
+        INTERFACE_INCLUDE_DIRECTORIES "${EtherCAT_INCLUDE_DIR}"
+        IMPORTED_LOCATION "${EtherCAT_LIBRARY}"
+    )
+endif()

dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.la

@@ -0,0 +1,41 @@
+# libethercat.la - a libtool library file
+# Generated by libtool (GNU libtool) 2.4.6 Debian-2.4.6-15build2
+#
+# Please DO NOT delete this file!
+# It is necessary for linking the library.
+
+# The name that we can dlopen(3).
+dlname='libethercat.so.1'
+
+# Names of this library.
+library_names='libethercat.so.1.2.0 libethercat.so.1 libethercat.so'
+
+# The name of the static archive.
+old_library='libethercat.a'
+
+# Linker flags that cannot go in dependency_libs.
+inherited_linker_flags=''
+
+# Libraries that this one depends upon.
+dependency_libs=''
+
+# Names of additional weak libraries provided by this library
+weak_library_names=''
+
+# Version information for libethercat.
+current=3
+age=2
+revision=0
+
+# Is this an already installed library?
+installed=yes
+
+# Should we warn about portability when linking against -modules?
+shouldnotlink=no
+
+# Files to dlopen/dlpreopen
+dlopen=''
+dlpreopen=''
+
+# Directory that this library needs to be installed in:
+libdir='/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/lib'

dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so

@@ -0,0 +1 @@
+libethercat.so.1.2.0

dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so.1

@@ -0,0 +1 @@
+libethercat.so.1.2.0

dependency/x86/third_party/ethercat/v1.7.0/lib/pkgconfig/libethercat.pc

@@ -0,0 +1,34 @@
+#
+#  pkgconfig file for ethercat library
+#
+#  Copyright 2021 Bjarne von Horn (vh at igh dot de)
+#
+#  This file is part of the ethercat library.
+#
+#  The ethercat library is free software: you can redistribute it and/or modify
+#  it under the terms of the GNU Lesser General Public License as published by
+#  the Free Software Foundation, either version 3 of the License, or (at your
+#  option) any later version.
+#
+#  The ethercat library is distributed in the hope that it will be useful, but
+#  WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+#  or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public
+#  License for more details.
+#
+#  You should have received a copy of the GNU Lesser General Public License
+#  along with the ethercat library. If not, see <http://www.gnu.org/licenses/>.
+#
+#  vim: tw=78 noexpandtab
+#
+
+prefix=/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0
+exec_prefix=${prefix}
+libdir=/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/lib
+includedir=/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/include
+
+Name: libethercat
+Description: Client support library for the EtherCAT Master
+URL: http://www.etherlab.org
+Version: 1.7.0
+Libs:  -L${libdir} -lethercat
+Cflags: -I${includedir}

dependency/x86/third_party/ethercat/v1.7.0/lib/systemd/system/ethercat.service

@@ -0,0 +1,38 @@
+#
+# EtherCAT master kernel modules
+#
+
+[Unit]
+Description=EtherCAT Master Kernel Modules
+
+# Fine tuning of the startup dependencies below are recommended
+# to provide a reliable startup routine.
+# The dependencies below can be either uncommented after copying
+# this file to /etc/systemd/system or by creating overrides:
+# Copy the needed dependencies into
+# /etc/systemd/system/ethercat.service.d/50-dependencies.conf
+# in a [Unit] section.
+
+#
+# Uncomment this, if the generic Ethernet driver is used. It assures, that the
+# network interfaces are configured, before the master starts.
+#
+#Requires=network.target # Stop master, if network is stopped
+#After=network.target # Start master, after network is ready
+
+#
+# Uncomment this, if a native Ethernet driver is used. It assures, that the
+# network interfaces are configured, after the Ethernet drivers have been
+# replaced. Otherwise, the networking configuration tools could be confused.
+#
+#Before=network-pre.target
+#Wants=network-pre.target
+
+[Service]
+Type=oneshot
+RemainAfterExit=yes
+ExecStart=/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/sbin/ethercatctl start
+ExecStop=/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/sbin/ethercatctl stop
+
+[Install]
+WantedBy=multi-user.target

dependency/x86/third_party/ethercat/v1.7.0/sbin/ethercatctl

@@ -0,0 +1,253 @@
+#!/bin/bash
+
+#------------------------------------------------------------------------------
+#
+#  Start script for EtherCAT to use with systemd
+#
+#  Copyright (C) 2006-2021  Florian Pose, Ingenieurgemeinschaft IgH
+#
+#  This file is part of the IgH EtherCAT Master.
+#
+#  The IgH EtherCAT Master is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License version 2, as
+#  published by the Free Software Foundation.
+#
+#  The IgH EtherCAT Master is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General
+#  Public License for more details.
+#
+#  You should have received a copy of the GNU General Public License along
+#  with the IgH EtherCAT Master; if not, write to the Free Software
+#  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+#
+#  vim: expandtab sw=4 tw=78
+#
+#------------------------------------------------------------------------------
+
+LSMOD="/sbin/lsmod"
+MODPROBE="/sbin/modprobe"
+RMMOD="/sbin/rmmod"
+MODINFO="/sbin/modinfo"
+IP="/sbin/ip"
+
+ETHERCAT="/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/bin/ethercat"
+
+#------------------------------------------------------------------------------
+
+if [ "$1" = "-c" ]; then
+    ETHERCAT_CONFIG="$2"
+    COMMAND="$3"
+else
+    ETHERCAT_CONFIG="/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf"
+    COMMAND="$1"
+fi
+
+#------------------------------------------------------------------------------
+
+if [ ! -r ${ETHERCAT_CONFIG} ]; then
+    echo ${ETHERCAT_CONFIG} not existing;
+    exit 6
+fi
+
+# shellcheck source=/etc/ethercat.conf
+. ${ETHERCAT_CONFIG}
+
+#------------------------------------------------------------------------------
+
+is_mac_address() {
+    local x='[0-9a-fA-F]'
+    echo "$1" | grep -qE "^($x$x:){5}$x$x\$" -
+}
+
+#------------------------------------------------------------------------------
+
+parse_mac_address() {
+    local DEVICENAMETOMAC
+    if [ -z "${1}" ] || is_mac_address "${1}"; then
+        MAC="${1}"
+    else
+        DEVICENAMETOMAC=$("${IP}" address show dev "${1}" |
+            awk '/link\/ether/ { print $2; }')
+        if is_mac_address "${DEVICENAMETOMAC}"; then
+            MAC="${DEVICENAMETOMAC}"
+        else
+            echo Invalid MAC address or interface name \""${1}"\" \
+                in ${ETHERCAT_CONFIG}
+            exit 1
+        fi
+    fi
+}
+
+#------------------------------------------------------------------------------
+
+case "$COMMAND" in
+
+start)
+    # bring up all updown interfaces before anything else
+    for interface in $UPDOWN_INTERFACES; do
+        $IP link set dev $interface up
+    done
+
+    # construct DEVICES and BACKUPS from configuration variables
+    DEVICES=""
+    BACKUPS=""
+    MASTER_INDEX=0
+
+    while true; do
+        DEVICE=$(eval echo "\${MASTER${MASTER_INDEX}_DEVICE}")
+        BACKUP=$(eval echo "\${MASTER${MASTER_INDEX}_BACKUP}")
+        if [ -z "${DEVICE}" ]; then break; fi
+
+        if [ ${MASTER_INDEX} -gt 0 ]; then
+            DEVICES=${DEVICES},
+            BACKUPS=${BACKUPS},
+        fi
+
+        parse_mac_address "${DEVICE}"
+        DEVICES=${DEVICES}${MAC}
+
+        parse_mac_address "${BACKUP}"
+        BACKUPS=${BACKUPS}${MAC}
+
+        MASTER_INDEX=$((${MASTER_INDEX} + 1))
+    done
+
+    if [ -z "${DEVICES}" ]; then
+        echo "ERROR: No network cards for EtherCAT specified."
+        echo -n "Please edit ${ETHERCAT_CONFIG} with root permissions"
+        echo -n " and set MASTER0_DEVICE variable to either a "
+        echo "network interface name (like eth0) or to a MAC address."
+        exit 1
+    fi
+
+    MODULE_PARAMS=(
+        main_devices="${DEVICES}"
+        backup_devices="${BACKUPS}"
+    )
+
+    if [ -n "$SII_CACHING" ]; then
+        MODULE_PARAMS+=(sii_caching="$SII_CACHING")
+    fi
+
+    # load master module
+    if ! ${MODPROBE} ${MODPROBE_FLAGS} ec_master "${MODULE_PARAMS[@]}"; then
+        exit 1
+    fi
+
+    LOADED_MODULES=ec_master
+
+    # check for modules to replace
+    for MODULE in ${DEVICE_MODULES}; do
+        ECMODULE=ec_${MODULE}
+        if ! ${MODINFO} "${ECMODULE}" > /dev/null; then
+            continue # ec_* module not found
+        fi
+
+        if [ "${MODULE}" != "generic" ] && [ "${MODULE}" != "ccat" ]; then
+            # unload standard module and check if unloading was successful
+            ${RMMOD} "${MODULE}" 2> /dev/null || true
+            if ${LSMOD} | grep "^${MODULE//-/_} " > /dev/null; then
+                # could not unload module
+                ${RMMOD} ${LOADED_MODULES}
+                exit 1
+            fi
+        fi
+
+        if ! ${MODPROBE} ${MODPROBE_FLAGS} "${ECMODULE}"; then
+            if [ "${MODULE}" != "generic" ] && [ "${MODULE}" != "ccat" ]; then
+                ${MODPROBE} ${MODPROBE_FLAGS} "${MODULE}" # try to restore
+            fi
+            ${RMMOD} ${LOADED_MODULES}
+            exit 1
+        fi
+
+        LOADED_MODULES="${ECMODULE} ${LOADED_MODULES}"
+    done
+
+    exit 0
+    ;;
+
+#------------------------------------------------------------------------------
+
+stop)
+    # unload EtherCAT device modules
+    for MODULE in ${DEVICE_MODULES} master; do
+        ECMODULE=ec_${MODULE}
+        if ! ${LSMOD} | grep -q "^${ECMODULE//-/_} "; then
+            continue # ec_* module not loaded
+        fi
+        if ! ${RMMOD} "${ECMODULE}"; then
+            exit 1
+        fi;
+    done
+
+    sleep 1
+
+    # load standard modules again
+    for MODULE in ${DEVICE_MODULES}; do
+        if [ "${MODULE}" == "generic" ] || [ "${MODULE}" == "ccat" ]; then
+            continue
+        fi
+        ${MODPROBE} ${MODPROBE_FLAGS} "${MODULE}"
+    done
+
+    # bring down all updown interfaces
+    for interface in $UPDOWN_INTERFACES; do
+        $IP link set dev $interface down
+    done
+
+    exit 0
+    ;;
+
+#------------------------------------------------------------------------------
+
+restart)
+    $0 stop || exit 1
+    sleep 1
+    $0 start
+    ;;
+
+#------------------------------------------------------------------------------
+
+status)
+    echo "Checking for EtherCAT master 1.7.0 "
+
+    # count masters in configuration file
+    MASTER_COUNT=0
+    while true; do
+        DEVICE=$(eval echo "\${MASTER${MASTER_COUNT}_DEVICE}")
+        if [ -z "${DEVICE}" ]; then break; fi
+        MASTER_COUNT=$((${MASTER_COUNT} + 1))
+    done
+
+    RESULT=0
+
+    for i in $(seq 0 "$((${MASTER_COUNT} - 1))"); do
+        echo -n "Master${i} "
+
+        # Check if the master is in idle or operation phase
+        ${ETHERCAT} master --master "${i}" 2>/dev/null | \
+            grep -qE 'Phase:[[:space:]]*Idle|Phase:[[:space:]]*Operation'
+        EXITCODE=$?
+
+        if [ ${EXITCODE} -eq 0 ]; then
+            echo " running"
+        else
+            echo " dead"
+            RESULT=1
+        fi
+    done
+
+    exit ${RESULT}
+    ;;
+
+#------------------------------------------------------------------------------
+
+*)
+    echo "USAGE: $0 [-c path/to/ethercat.conf] {start|stop|restart|status}"
+    exit 1
+    ;;
+esac
+
+#------------------------------------------------------------------------------

dependency/x86/third_party/ethercat/v1.7.0/share/bash-completion/completions/ethercat

@@ -0,0 +1,80 @@
+#  Copyright (C) 2022 Bjarne von Horn, Ingenieurgemeinschaft IgH
+#
+#  This file is part of the IgH EtherCAT master userspace library.
+#
+#  The IgH EtherCAT master userspace library is free software; you can
+#  redistribute it and/or modify it under the terms of the GNU Lesser General
+#  Public License as published by the Free Software Foundation; version 2.1
+#  of the License.
+#
+#  The IgH EtherCAT master userspace library is distributed in the hope that
+#  it will be useful, but WITHOUT ANY WARRANTY; without even the implied
+#  warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#  GNU Lesser General Public License for more details.
+#
+#  You should have received a copy of the GNU Lesser General Public License
+#  along with the IgH EtherCAT master userspace library. If not, see
+#  <http://www.gnu.org/licenses/>.
+#
+
+_ethercat_completions()
+{
+    local ethercat_commands="alias config crc cstruct data debug domains download eoe foe_read foe_write graph master pdos reg_read reg_write rescan sdos sii_read sii_write slaves soe_read soe_write states upload version xml"
+    local options="--help --force --quiet --verbose --master "
+    if [ "$COMP_CWORD" -eq 1 ] ; then
+        COMPREPLY=($(compgen -W "$ethercat_commands --help" -- "${COMP_WORDS[1]}"))
+    elif [[ "${COMP_WORDS[1]}" != "--help" && ! "${COMP_WORDS[COMP_CWORD-1]}" =~ ^-a|-p|--alias|--position$ ]] ; then
+        case "${COMP_WORDS[1]}" in
+        "alias" | "config" | "cstruct" | "slaves" | "sdos" | "sii_read" | "upload" | "xml")
+            options+="--alias --position"
+            ;;
+        "crc")
+            options+="reset"
+            ;;
+        "debug")
+            options+="0 1 2"
+            ;;
+        "domains")
+            options+="--domain"
+            ;;
+        "download" | "reg_read" | "soe_read" | "soe_write")
+            if [[ "${COMP_WORDS[COMP_CWORD-1]}" =~ ^-t|--type$ ]] ; then
+                options="bool int8 int16 int32 int64 uint8 uint16 uint32 uint64 float double string octet_string unicode_string sm8 sm16 sm32 sm64"
+            else
+                options+="--alias --position --type"
+            fi
+            ;;
+        "foe_read" | "foe_write")
+            if [[  "${COMP_WORDS[COMP_CWORD-1]}" =~ ^-o|--output-file$ ]] ; then
+                COMPREPLY=($(compgen -o filenames -A file -- "${COMP_WORDS[$COMP_CWORD]}"))
+            else
+                options+="--alias --position --output-file"
+                COMPREPLY=($(compgen -o filenames -A file -W "$options" -- "${COMP_WORDS[$COMP_CWORD]}"))
+            fi
+            return
+            ;;
+        "graph")
+            options+="DC CRC"
+            ;;
+        "pdos")
+            if [[ "${COMP_WORDS[COMP_CWORD-1]}" =~ ^-s|--skin$ ]] ; then
+                options="default etherlab"
+            else
+                options+="--alias --position --skin"
+            fi
+            ;;
+        "sii_write")
+            options+="--alias --position"
+            COMPREPLY=($(compgen -o filenames -A file -W "$options" -- "${COMP_WORDS[$COMP_CWORD]}"))
+            return
+            ;;
+        "states")
+            options+="--alias --position INIT PREOP BOOT SAFEOP OP"
+            ;;
+
+        esac
+        COMPREPLY+=($(compgen -W "$options" -- "${COMP_WORDS[$COMP_CWORD]}"))
+    fi
+}
+
+complete -F _ethercat_completions ethercat

docs/ethercat_motor_tutorial.md

@@ -1,25 +1,51 @@
 # EtherCAT 电机接入教程
 
-这份文档只说明新增一种 EtherCAT 电机需要改哪里、怎么写。
+当前已接入意优 `EYOU_ServoModule_ECAT_V145`，协议为 EtherCAT CoE + CiA402，PDO 使用 `docs/EYOU_ServoModule_ECAT_V145_no_slot.xml` 中的 `0x1600/0x1A00` 映射。
 
-## 1. 增加 vendor
+ESI XML 作为厂商通信说明和对照资料保存，运行时不直接解析 XML。实际 PDO 映射写在：
 
-修改 `protos/cmvr/config/motor_config/motor_config.proto`：
-
-```proto
-enum MotorVendor {
-  MOTOR_VENDOR_UNKNOWN = 0;
-  MOTOR_VENDOR_TI5 = 1;
-  MOTOR_VENDOR_MUJOCO = 2;
-  MOTOR_VENDOR_XXX = 3;
-}
+```text
+cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h
 ```
 
-`MOTOR_VENDOR_XXX` 改成真实厂商名，例如 `MOTOR_VENDOR_FOO`。不要复用 `TI5`。
+## 1. 系统依赖
 
-## 2. 写电机配置
+IgH EtherCAT userspace 已安装在：
 
-新增配置文件：
+```text
+dependency/x86/third_party/ethercat/v1.7.0
+```
+
+真机运行前，系统里还需要安装/加载 IgH master 内核模块。使用仓库脚本启动 EtherCAT master：
+
+```bash
+sudo script/ethercat/start_ethercat.sh eno1
+script/ethercat/status_ethercat.sh
+```
+
+其中 `eno1` 是连接 EtherCAT 从站的网卡。脚本会读取该网卡 MAC，写入：
+
+```text
+dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf
+```
+
+并通过 bundled IgH 的 `ethercatctl -c` 启动 master。能看到 EYOU slave 后再启动程序。
+
+停止 EtherCAT：
+
+```bash
+sudo script/ethercat/stop_ethercat.sh eno1
+```
+
+如果这张网卡要恢复给普通网络使用：
+
+```bash
+sudo script/ethercat/stop_ethercat.sh eno1 --restore-network
+```
+
+## 2. 电机配置
+
+新增或修改：
 
 ```text
 cmvr-es/config/devices/motor/ethercat_motors.pb.txt
@@ -34,20 +60,28 @@ motor {
   motor_groups {
     id: "right_arm_ethercat"
     bus_type: MOTOR_BUS_ETHERCAT
-    vendor: MOTOR_VENDOR_XXX
+    vendor: MOTOR_VENDOR_EYOU
     protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402
-    tool_frame: "R_FINGER_TIP"
 
     ethercat {
-      master_id: "eth0"
+      master_index: 0
       cycle_us: 1000
-      slaves { motor_id: 1 slave_index: 0 vendor_id: 0x00000000 product_code: 0x00000000 }
-      slaves { motor_id: 2 slave_index: 1 vendor_id: 0x00000000 product_code: 0x00000000 }
-      slaves { motor_id: 3 slave_index: 2 vendor_id: 0x00000000 product_code: 0x00000000 }
-      slaves { motor_id: 4 slave_index: 3 vendor_id: 0x00000000 product_code: 0x00000000 }
-      slaves { motor_id: 5 slave_index: 4 vendor_id: 0x00000000 product_code: 0x00000000 }
-      slaves { motor_id: 6 slave_index: 5 vendor_id: 0x00000000 product_code: 0x00000000 }
-      slaves { motor_id: 7 slave_index: 6 vendor_id: 0x00000000 product_code: 0x00000000 }
+
+      cia402 {
+        profile_position_trigger_delay_ms: 2
+        state_transition_timeout_ms: 1200
+        velocity_stop_timeout_ms: 2000
+        status_poll_period_ms: 10
+        stopped_velocity_tolerance_rad_s: 0.001
+      }
+
+      slaves { motor_id: 1 alias: 0 position: 0 }
+      slaves { motor_id: 2 alias: 0 position: 1 }
+      slaves { motor_id: 3 alias: 0 position: 2 }
+      slaves { motor_id: 4 alias: 0 position: 3 }
+      slaves { motor_id: 5 alias: 0 position: 4 }
+      slaves { motor_id: 6 alias: 0 position: 5 }
+      slaves { motor_id: 7 alias: 0 position: 6 }
     }
 
     joint_limits {
@@ -63,21 +97,26 @@ motor {
     }
 
     motors {
-      motors { id: 1 joint_name: "R_SHOULDER_P" }
-      motors { id: 2 joint_name: "R_SHOULDER_R" }
-      motors { id: 3 joint_name: "R_SHOULDER_Y" }
-      motors { id: 4 joint_name: "R_ELBOW_R" }
-      motors { id: 5 joint_name: "R_WRIST_P" }
-      motors { id: 6 joint_name: "R_WRIST_Y" }
-      motors { id: 7 joint_name: "R_WRIST_R" }
+      motors { id: 1 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 2 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 3 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 4 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 5 joint_name: "R_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 6 joint_name: "R_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
+      motors { id: 7 joint_name: "R_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
     }
   }
 }
 ```
 
-`motors.motors.id` 是系统内的电机逻辑 id。`ethercat.slaves.motor_id` 必须和它对应。
+字段说明：
 
-`joint_limits` 按 `joint_name` 读取，和 CAN、MuJoCo 电机保持同一个风格。
+- `master_index`：IgH master 编号，第一张 EtherCAT master 是 `0`。
+- `alias`：EtherCAT alias，一般为 `0`。
+- `position`：slave 在 EtherCAT 链路上的顺序，从 `0` 开始。
+- `motor_id`：系统内电机 id，必须和 `motors.motors.id` 对应。
+- `encoder_counts_per_rev`：电机编码器每转 count 数，用于 `rad <-> count` 换算。
+- `gear_ratio`：电机轴到关节输出轴的减速比，用于接口层 `rad/rad/s` 和驱动器 raw count/counts/s 换算。
 
 ## 3. 注册设备
 
@@ -98,15 +137,9 @@ devices {
 }
 ```
 
-## 4. 机械臂使用 EtherCAT group
+## 4. 机械臂使用 EtherCAT 电机
 
-修改机械臂配置，例如：
-
-```text
-cmvr-es/config/devices/arm/arm.pb.txt
-```
-
-把 motor backend 改成：
+修改机械臂配置中的 motor backend：
 
 ```proto
 motor {
@@ -122,116 +155,66 @@ motor {
 }
 ```
 
-## 5. 实现 bus runtime
+## 5. 代码结构
 
-EtherCAT 总线资源放在：
+EtherCAT 总线运行时：
 
 ```text
 cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h
 cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp
 ```
 
-`EthercatMotorBusRuntime` 负责：
+它负责：
+
+- 打开 IgH master
+- 创建 domain
+- 使用 `vendor + protocol` 选择 PDO mapping
+- 按 mapping 配置 `0x1600/0x1A00` PDO
+- 注册每个非 padding PDO entry 的 offset
+- 启动 cyclic loop
+- 按 `motor_id + index + subindex` 提供通用 PDO 读写接口
+
+CiA402 EtherCAT 电机驱动：
 
 ```text
-读取 ethercat 配置
-初始化 EtherCAT master
-扫描/校验 slave_index、vendor_id、product_code
-启动 cyclic loop
-保存 command/feedback buffer
-停止 cyclic loop
+cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h
+cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h
+cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h
+cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp
 ```
 
-bus runtime 不创建具体电机，也不关心厂商；它只保存总线连接、线程和数据缓存。
-
-## 6. 增加具体电机 driver
-
-新增目录：
+EYOU 私有适配：
 
 ```text
-cmvr-es/devices/motor/drivers/xxx_ethercat/
-  CMakeLists.txt
-  include/xxx_ethercat_motor.h
-  include/xxx_ethercat_motor_protocol.h
-  src/xxx_ethercat_motor.cpp
-  src/xxx_ethercat_motor_protocol.cpp
+cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h
+cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h
+cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h
+cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h
+cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp
+cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp
 ```
 
-`XxxEthercatMotor` 继承 `AbstractMotor`。
+它负责：
 
-`XxxEthercatMotorProtocol` 继承 `MotorProtocolInterface`，把 `setTarget`、`setQd`、`getQ` 等接口转换成 EtherCAT command/feedback。
+- CiA402 `6040/6041` 状态机
+- 设置 `6060` 运行模式
+- 写 `607A/60FF/6071`
+- 读 `6064/606C/6077/603F`
+- `rad` 和 encoder count 的换算
 
-## 7. 在 MotorManager 里创建 EtherCAT 电机
-
-修改：
-
-```text
-cmvr-es/devices/motor/manager/src/motor_manager.cpp
-```
-
-在 `MotorManager::createEthercatMotors_()` 里按 `vendor + protocol` 创建具体电机：
-
-```cpp
-auto ethercat_bus_runtime =
-    std::dynamic_pointer_cast<EthercatMotorBusRuntime>(bus_runtime);
-
-if (group_cfg.vendor() == config::MOTOR_VENDOR_XXX &&
-    group_cfg.protocol() == config::MOTOR_PROTOCOL_ETHERCAT_CIA402) {
-    auto protocol = std::make_shared<XxxEthercatMotorProtocol>(ethercat_bus_runtime);
-
-    std::vector<std::shared_ptr<AbstractMotor>> motors;
-    motors.reserve(motor_cfgs.size());
-    for (const auto& cfg : motor_cfgs) {
-        auto motor = std::make_shared<XxxEthercatMotor>(cfg);
-        motor->setProtocol(protocol);
-        if (!motor->init()) {
-            return {};
-        }
-        motors.push_back(std::move(motor));
-    }
-    return motors;
-}
-```
-
-## 8. 加入 CMake
-
-修改：
-
-```text
-cmvr-es/devices/motor/manager/CMakeLists.txt
-```
-
-给 `motor_manager` 链接新增的具体 EtherCAT 电机 target。
-
-修改：
-
-```text
-cmvr-es/devices/motor/CMakeLists.txt
-```
-
-加入：
-
-```cmake
-add_subdirectory(drivers/xxx_ethercat)
-```
-
-## 9. 验证配置
-
-先验证 proto：
+## 6. 编译验证
 
 ```bash
-./output/bin/protoc \
-  --encode=cmvr.config.MotorRootConfig \
-  -I protos \
-  protos/cmvr/config/motor_config/motor_config.proto \
-  < cmvr-es/config/devices/motor/ethercat_motors.pb.txt \
-  > /tmp/ethercat_motors.pb.bin
+cmake -S . -B cmake-build-debug
+cmake --build cmake-build-debug --target motor_manager
 ```
 
-再编译：
+真机验证顺序：
 
 ```bash
-cmake --build cmake-build-debug --target mujoco_manual_ui_test
+sudo script/ethercat/start_ethercat.sh eno1
+script/ethercat/status_ethercat.sh
+cmake --build cmake-build-debug --target cmvr_es
 ```
 
-真机联调时先只验证初始化日志：master 打开、slave 数量、vendor/product 校验、cyclic loop 启动、每个 motor 注册成功。然后再下发运动命令。
+第一次联调先不要大幅度运动。先看 master 是否打开、slave 是否进入 OP、状态字是否更新，再给单个电机小角度目标。

protos/cmvr/config/device_manager_config/device_manager_config.proto

@@ -34,6 +34,7 @@ message DeviceManagerConfig {
   string version = 2;
   string description = 3;
   repeated DeviceConfigEntry devices = 4;
+  bool init_all_motors_when_no_active_joints = 20;
 }
 message DeviceManagerRootConfig {
   DeviceManagerConfig device_manager = 1;

protos/cmvr/config/motor_config/motor_config.proto

@@ -10,6 +10,8 @@ message MotorConfigItem {
   double limit_q_ub = 4;
   double limit_qd = 5;
   double limit_qdd = 6;
+  double encoder_counts_per_rev = 7;
+  double gear_ratio = 8;
 }
 
 message MotorList {
@@ -18,9 +20,26 @@ message MotorList {
 
 message EthercatSlaveConfig {
   int32 motor_id = 1;
-  int32 slave_index = 2;
-  uint32 vendor_id = 3;
-  uint32 product_code = 4;
+  uint32 alias = 2;
+  uint32 position = 3;
+}
+
+message Cia402ProtocolConfig {
+  uint32 profile_position_trigger_delay_ms = 1;
+  uint32 state_transition_timeout_ms = 2;
+  uint32 velocity_stop_timeout_ms = 3;
+  uint32 status_poll_period_ms = 4;
+  double stopped_velocity_tolerance_rad_s = 5;
+}
+
+message EtherCATDcConfig {
+  optional bool enable = 1;
+  optional int32 reference_motor_id = 2;
+  optional uint32 sync0_cycle_us = 3;
+  optional int32 sync0_shift_us = 4;
+  optional uint32 sync_reference_clock_period = 5;
+  optional uint32 assign_activate = 6;
+  optional uint32 sync_monitor_period_ms = 7;
 }
 
 message SocketCanConfig {
@@ -29,8 +48,12 @@ message SocketCanConfig {
 }
 
 message EtherCATConfig {
-  string master_id = 1;
+  uint32 master_index = 1;
   int32 cycle_us = 2;
+  Cia402ProtocolConfig cia402 = 3;
+  EtherCATDcConfig dc = 4;
+  optional uint32 slave_op_timeout_ms = 5;
+  optional uint32 slave_state_poll_period_ms = 6;
   repeated EthercatSlaveConfig slaves = 10;
 }
 
@@ -49,6 +72,7 @@ enum MotorVendor {
   MOTOR_VENDOR_UNKNOWN = 0;
   MOTOR_VENDOR_TI5 = 1;
   MOTOR_VENDOR_MUJOCO = 2;
+  MOTOR_VENDOR_EYOU = 3;
 }
 
 enum MotorProtocol {
@@ -63,7 +87,6 @@ message MotorGroupConfig {
   MotorBusType bus_type = 2;
   MotorVendor vendor = 3;
   MotorProtocol protocol = 4;
-  string tool_frame = 5;
 
   oneof bus_config {
     SocketCanConfig can = 10;

protos/cmvr/msgs/canopen.proto

@@ -5,8 +5,8 @@ package cmvr.msgs;
 message SdoFrame {
   uint32 node_id = 1;            // 节点ID
   CommandSpecifier cs = 2;  // SDO命令字
-  ObIndex index = 3;              // 对象字典索引
-  ObSubIndex sub_index = 4;          // 子索引
+  uint32 index = 3;              // 对象字典索引
+  uint32 sub_index = 4;          // 子索引
   uint32 data = 5;                // 数据区
 }
 
@@ -110,81 +110,38 @@ enum NmtCommand {
 }
 
 
-// 索引
-enum ObIndex {
-  INDEX_ZERO = 0;
-  USER_SAVE_PARA_2000 = 0x2000;         // 下发命令 1 保存参数
-  POSITION_OFFSET_2008 = 0x2008;  // 位置偏移，子索引 0x00，用于设置零点、起始位置
-  // Error Codes
-  ERROR_CODE_6007 = 0x6007;
-  ERROR_CODE_603F = 0x603F;
+// CANopen communication object dictionary indexes.
+// CiA402 drive-profile objects are defined in cia402.proto.
+enum CanopenObjectIndex {
+  CANOPEN_OBJECT_INDEX_ZERO = 0;
 
-  // Control and Status
-  CONTROL_WORD_6040 = 0x6040;
-  STATUS_WORD_6041 = 0x6041;
+  CANOPEN_PRODUCER_HEARTBEAT_TIME_1017 = 0x1017;
 
-  // Operation Modes
-  OPERATION_MODE_6060 = 0x6060;
-  MODE_DISPLAY_6061 = 0x6061;
-
-  // Actual Values
-  ACTUAL_POSITION_6064 = 0x6064;
-  ACTUAL_SPEED_606C = 0x606C;
-  ACTUAL_CURRENT_6078 = 0x6078;
-
-  // Torque-related
-  TARGET_TORQUE_6071 = 0x6071;
-  MAX_TORQUE_6072 = 0x6072;
-  DEMAND_TORQUE_6074 = 0x6074;
-
-  // Position-related
-  TARGET_POSITION_607A = 0x607A;
-  SOFTWARE_POSITION_LIMIT_607D = 0x607D;  // Sub-indexes: 1, 2
-
-  // Speed-related
-  MAX_SPEED_607F = 0x607F;
-  PROFILE_SPEED_6081 = 0x6081;
-  PROFILE_ACCELERATION_6083 = 0x6083;
-  PROFILE_DECELERATION_6084 = 0x6084;
-
-  // Same as DEMAND_TORQUE? Verify correctness.
-  //  TORQUE_SLOPE_6074 = 0x6074;
-
-  // PID Control
-  CURRENT_LOOP_PID_60F6 = 0x60F6;  // Sub-indexes: 1, 2
-  SPEED_LOOP_PID_60F9 = 0x60F9;    // Sub-indexes: 1, 2
-  POSITION_LOOP_PID_60FB = 0x60FB;  // Sub-indexes: 1, 2, 3
-
-  // Target Speed
-  TARGET_SPEED_60FF = 0x60FF;
-
-  QUICK_STOP_OPTION_605A = 0x605A;
-  QUICK_STOP_DECEL_6085 = 0x6085;
-
-  // -------------------------
   // PDO 通信参数对象（Communication Object）
-  RPDO1_COMM_1400 = 0x1400;
-  RPDO2_COMM_1401 = 0x1401;
-  RPDO3_COMM_1402 = 0x1402;
-  RPDO4_COMM_1403 = 0x1403;
+  CANOPEN_RPDO1_COMM_1400 = 0x1400;
+  CANOPEN_RPDO2_COMM_1401 = 0x1401;
+  CANOPEN_RPDO3_COMM_1402 = 0x1402;
+  CANOPEN_RPDO4_COMM_1403 = 0x1403;
 
-  TPDO1_COMM_1800 = 0x1800;
-  TPDO2_COMM_1801 = 0x1801;
-  TPDO3_COMM_1802 = 0x1802;
-  TPDO4_COMM_1803 = 0x1803;
+  CANOPEN_TPDO1_COMM_1800 = 0x1800;
+  CANOPEN_TPDO2_COMM_1801 = 0x1801;
+  CANOPEN_TPDO3_COMM_1802 = 0x1802;
+  CANOPEN_TPDO4_COMM_1803 = 0x1803;
 
   // PDO 映射对象（Mapping Object）
-  RPDO1_MAP_1600 = 0x1600;
-  RPDO2_MAP_1601 = 0x1601;
-  RPDO3_MAP_1602 = 0x1602;
-  RPDO4_MAP_1603 = 0x1603;
+  CANOPEN_RPDO1_MAP_1600 = 0x1600;
+  CANOPEN_RPDO2_MAP_1601 = 0x1601;
+  CANOPEN_RPDO3_MAP_1602 = 0x1602;
+  CANOPEN_RPDO4_MAP_1603 = 0x1603;
 
-  TPDO1_MAP_1A00 = 0x1A00;
-  TPDO2_MAP_1A01 = 0x1A01;
-  TPDO3_MAP_1A02 = 0x1A02;
-  TPDO4_MAP_1A03 = 0x1A03;
+  CANOPEN_TPDO1_MAP_1A00 = 0x1A00;
+  CANOPEN_TPDO2_MAP_1A01 = 0x1A01;
+  CANOPEN_TPDO3_MAP_1A02 = 0x1A02;
+  CANOPEN_TPDO4_MAP_1A03 = 0x1A03;
 
-  PRODUCER_HEARTBEAT_TIME = 0x1017;
+  // Ti5 vendor-specific objects used through CANopen SDO.
+  CANOPEN_USER_SAVE_PARA_2000 = 0x2000;
+  CANOPEN_POSITION_OFFSET_2008 = 0x2008;
 }
 
 // 子索引
@@ -198,5 +155,3 @@ enum ObSubIndex {
   SUB_INDEX_6 = 6;
   SUB_INDEX_7 = 7;
 }
-
-

protos/cmvr/msgs/cia402.proto

@@ -0,0 +1,64 @@
+syntax = "proto3";
+
+package cmvr.msgs;
+
+// CiA402 object dictionary indexes shared by CANopen and EtherCAT CoE drives.
+enum Cia402ObjectIndex {
+  CIA402_OBJECT_INDEX_ZERO = 0;
+
+  CIA402_ERROR_CODE_603F = 0x603F;
+
+  CIA402_CONTROL_WORD_6040 = 0x6040;
+  CIA402_STATUS_WORD_6041 = 0x6041;
+
+  CIA402_QUICK_STOP_OPTION_605A = 0x605A;
+  CIA402_SHUTDOWN_OPTION_605B = 0x605B;
+  CIA402_DISABLE_OPERATION_OPTION_605C = 0x605C;
+  CIA402_HALT_OPTION_605D = 0x605D;
+  CIA402_FAULT_REACTION_OPTION_605E = 0x605E;
+
+  CIA402_OPERATION_MODE_6060 = 0x6060;
+  CIA402_MODE_DISPLAY_6061 = 0x6061;
+
+  CIA402_POSITION_DEMAND_VALUE_6062 = 0x6062;
+  CIA402_ACTUAL_POSITION_6064 = 0x6064;
+  CIA402_MAX_FOLLOWING_ERROR_6065 = 0x6065;
+  CIA402_POSITION_WINDOW_6067 = 0x6067;
+  CIA402_POSITION_WINDOW_TIME_6068 = 0x6068;
+
+  CIA402_VELOCITY_DEMAND_VALUE_606B = 0x606B;
+  CIA402_ACTUAL_VELOCITY_606C = 0x606C;
+  CIA402_VELOCITY_WINDOW_606D = 0x606D;
+  CIA402_VELOCITY_WINDOW_TIME_606E = 0x606E;
+  CIA402_VELOCITY_THRESHOLD_606F = 0x606F;
+  CIA402_VELOCITY_THRESHOLD_TIME_6070 = 0x6070;
+
+  CIA402_TARGET_TORQUE_6071 = 0x6071;
+  CIA402_MAX_TORQUE_6072 = 0x6072;
+  CIA402_TORQUE_DEMAND_VALUE_6074 = 0x6074;
+  CIA402_MOTOR_RATED_TORQUE_6076 = 0x6076;
+  CIA402_ACTUAL_TORQUE_6077 = 0x6077;
+  CIA402_ACTUAL_CURRENT_6078 = 0x6078;
+  CIA402_DC_LINK_VOLTAGE_6079 = 0x6079;
+
+  CIA402_TARGET_POSITION_607A = 0x607A;
+  CIA402_HOME_OFFSET_607C = 0x607C;
+  CIA402_SOFTWARE_POSITION_LIMIT_607D = 0x607D;
+  CIA402_MAX_PROFILE_VELOCITY_607F = 0x607F;
+
+  CIA402_PROFILE_VELOCITY_6081 = 0x6081;
+  CIA402_PROFILE_ACCELERATION_6083 = 0x6083;
+  CIA402_PROFILE_DECELERATION_6084 = 0x6084;
+  CIA402_QUICK_STOP_DECELERATION_6085 = 0x6085;
+  CIA402_TORQUE_SLOPE_6087 = 0x6087;
+
+  CIA402_GEAR_RATIO_6091 = 0x6091;
+  CIA402_VELOCITY_OFFSET_60B1 = 0x60B1;
+  CIA402_TORQUE_OFFSET_60B2 = 0x60B2;
+  CIA402_INTERPOLATION_DATA_RECORD_60C1 = 0x60C1;
+  CIA402_INTERPOLATION_TIME_PERIOD_60C2 = 0x60C2;
+  CIA402_FOLLOWING_ERROR_ACTUAL_VALUE_60F4 = 0x60F4;
+  CIA402_TARGET_VELOCITY_60FF = 0x60FF;
+
+  CIA402_SUPPORTED_DRIVE_MODES_6502 = 0x6502;
+}

protos/cmvr/msgs/motor.proto

@@ -114,5 +114,3 @@ message MotorStatus {
   uint32  status_word = 44;
 
 }
-
-

protos/cmvr/msgs/robot_detail.proto

@@ -1,6 +1,5 @@
 syntax = "proto3";
 
-import "cmvr/msgs/canopen.proto";
 import "cmvr/msgs/motor.proto";
 package cmvr.msgs;
 

request.txt

@@ -33,5 +33,6 @@ third_party/modbus/3.1.11
 third_party/visp/3.7.0
 third_party/mainif/0.0.5
 third_party/matplotplusplus/1.2.0
+third_party/ethercat/v1.7.0
 
 

script/ethercat/start_ethercat.sh

@@ -0,0 +1,133 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+usage() {
+  cat <<'USAGE'
+Usage:
+  sudo script/ethercat/start_ethercat.sh [iface] [ethercat_dev] [start_wait_sec]
+  Any pure numeric argument is treated as start_wait_sec.
+
+Example:
+  sudo script/ethercat/start_ethercat.sh eno1
+  sudo script/ethercat/start_ethercat.sh 10
+  sudo script/ethercat/start_ethercat.sh eno1 10
+  sudo script/ethercat/start_ethercat.sh eno1 /dev/EtherCAT1
+  sudo script/ethercat/start_ethercat.sh eno1 /dev/EtherCAT0 10
+  sudo script/ethercat/start_ethercat.sh eno1 10 /dev/EtherCAT0
+
+Environment:
+  DEVICE_MODULES=generic       IgH device module list.
+  IGH_ROOT=...                 Override bundled IgH install path.
+  ETHERCAT_CONF=...            Override ethercatctl config path.
+  ETHERCAT_DEV=/dev/EtherCAT0  Override EtherCAT character device node.
+  START_WAIT_SEC=5             Seconds to wait for link/slave discovery.
+USAGE
+}
+
+if [[ "${1:-}" == "-h" || "${1:-}" == "--help" ]]; then
+  usage
+  exit 0
+fi
+
+if [[ "$(id -u)" -ne 0 ]]; then
+  echo "error: please run with sudo." >&2
+  exit 1
+fi
+
+IFACE="${IFACE:-eno1}"
+ETHERCAT_DEV="${ETHERCAT_DEV:-/dev/EtherCAT0}"
+START_WAIT_SEC="${START_WAIT_SEC:-5}"
+
+NON_NUMERIC_ARG_COUNT=0
+for arg in "$@"; do
+  if [[ "${arg}" =~ ^[0-9]+$ ]]; then
+    START_WAIT_SEC="${arg}"
+    continue
+  fi
+
+  case "${NON_NUMERIC_ARG_COUNT}" in
+    0)
+      IFACE="${arg}"
+      ;;
+    1)
+      ETHERCAT_DEV="${arg}"
+      ;;
+    *)
+      echo "error: unexpected argument: ${arg}" >&2
+      usage >&2
+      exit 1
+      ;;
+  esac
+  NON_NUMERIC_ARG_COUNT=$((NON_NUMERIC_ARG_COUNT + 1))
+done
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+REPO_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)"
+IGH_ROOT="${IGH_ROOT:-${REPO_ROOT}/dependency/x86/third_party/ethercat/v1.7.0}"
+DEVICE_MODULES="${DEVICE_MODULES:-generic}"
+ETHERCAT_CONF="${ETHERCAT_CONF:-${IGH_ROOT}/etc/ethercat.conf}"
+ETHERCATCTL="${IGH_ROOT}/sbin/ethercatctl"
+ETHERCAT="${IGH_ROOT}/bin/ethercat"
+
+if [[ ! -d "/sys/class/net/${IFACE}" ]]; then
+  echo "error: network interface '${IFACE}' does not exist." >&2
+  exit 1
+fi
+
+if [[ ! -x "${ETHERCATCTL}" ]]; then
+  echo "error: ethercatctl not found: ${ETHERCATCTL}" >&2
+  exit 1
+fi
+
+if [[ ! -x "${ETHERCAT}" ]]; then
+  echo "error: ethercat command not found: ${ETHERCAT}" >&2
+  exit 1
+fi
+
+MAC="$(cat "/sys/class/net/${IFACE}/address")"
+
+echo "EtherCAT interface: ${IFACE}"
+echo "EtherCAT MAC: ${MAC}"
+echo "EtherCAT device: ${ETHERCAT_DEV}"
+echo "IgH root: ${IGH_ROOT}"
+echo "IgH config: ${ETHERCAT_CONF}"
+echo "Device modules: ${DEVICE_MODULES}"
+echo "Start wait: ${START_WAIT_SEC}s"
+
+mkdir -p "$(dirname "${ETHERCAT_CONF}")"
+
+if [[ -f "${ETHERCAT_CONF}" ]]; then
+  echo "Stopping existing EtherCAT master with current config..."
+  "${ETHERCATCTL}" -c "${ETHERCAT_CONF}" stop >/dev/null 2>&1 || true
+  sleep 1
+fi
+
+cat >"${ETHERCAT_CONF}" <<EOF
+MASTER0_DEVICE="${MAC}"
+DEVICE_MODULES="${DEVICE_MODULES}"
+UPDOWN_INTERFACES="${IFACE}"
+EOF
+
+if command -v nmcli >/dev/null 2>&1; then
+  nmcli device disconnect "${IFACE}" >/dev/null 2>&1 || true
+fi
+
+ip addr flush dev "${IFACE}"
+ip link set "${IFACE}" up
+
+"${ETHERCATCTL}" -c "${ETHERCAT_CONF}" start
+
+if [[ -e "${ETHERCAT_DEV}" ]]; then
+  chmod 666 "${ETHERCAT_DEV}"
+else
+  echo "warning: ${ETHERCAT_DEV} not found; skip chmod. Check with: ls -l /dev/EtherCAT*" >&2
+fi
+
+for ((i = 0; i < START_WAIT_SEC; ++i)); do
+  if "${ETHERCAT}" slaves 2>/dev/null | grep -qE '^[0-9]+[[:space:]]'; then
+    break
+  fi
+  sleep 1
+done
+
+"${ETHERCAT}" master
+"${ETHERCAT}" slaves || true

script/ethercat/status_ethercat.sh

@@ -0,0 +1,62 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+usage() {
+  cat <<'USAGE'
+Usage:
+  script/ethercat/status_ethercat.sh
+
+Environment:
+  IGH_ROOT=...         Override bundled IgH install path.
+  ETHERCAT_CONF=...    Override ethercatctl config path.
+USAGE
+}
+
+if [[ "${1:-}" == "-h" || "${1:-}" == "--help" ]]; then
+  usage
+  exit 0
+fi
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+REPO_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)"
+IGH_ROOT="${IGH_ROOT:-${REPO_ROOT}/dependency/x86/third_party/ethercat/v1.7.0}"
+ETHERCAT_CONF="${ETHERCAT_CONF:-${IGH_ROOT}/etc/ethercat.conf}"
+ETHERCATCTL="${IGH_ROOT}/sbin/ethercatctl"
+ETHERCAT="${IGH_ROOT}/bin/ethercat"
+
+if [[ ! -x "${ETHERCATCTL}" ]]; then
+  echo "error: ethercatctl not found: ${ETHERCATCTL}" >&2
+  exit 1
+fi
+
+if [[ ! -x "${ETHERCAT}" ]]; then
+  echo "error: ethercat command not found: ${ETHERCAT}" >&2
+  exit 1
+fi
+
+echo "== ${ETHERCAT_CONF} =="
+if [[ -f "${ETHERCAT_CONF}" ]]; then
+  sed -n '1,80p' "${ETHERCAT_CONF}"
+else
+  echo "missing"
+fi
+
+echo
+echo "== kernel modules =="
+lsmod | grep -E '(^ec_master|^ec_generic|^ec_)' || true
+
+echo
+echo "== ethercatctl =="
+"${ETHERCATCTL}" -c "${ETHERCAT_CONF}" status || true
+
+echo
+echo "== master =="
+"${ETHERCAT}" master || true
+
+echo
+echo "== slaves =="
+"${ETHERCAT}" slaves || true
+
+echo
+echo "== pdos =="
+"${ETHERCAT}" pdos || true

script/ethercat/stop_ethercat.sh

@@ -0,0 +1,70 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+usage() {
+  cat <<'USAGE'
+Usage:
+  sudo script/ethercat/stop_ethercat.sh [iface] [--restore-network]
+
+Examples:
+  sudo script/ethercat/stop_ethercat.sh eno1
+  sudo script/ethercat/stop_ethercat.sh eno1 --restore-network
+
+Environment:
+  IGH_ROOT=...         Override bundled IgH install path.
+  ETHERCAT_CONF=...    Override ethercatctl config path.
+USAGE
+}
+
+if [[ "${1:-}" == "-h" || "${1:-}" == "--help" ]]; then
+  usage
+  exit 0
+fi
+
+if [[ "$(id -u)" -ne 0 ]]; then
+  echo "error: please run with sudo." >&2
+  exit 1
+fi
+
+IFACE="eno1"
+RESTORE_NETWORK="false"
+
+for arg in "$@"; do
+  case "${arg}" in
+    --restore-network)
+      RESTORE_NETWORK="true"
+      ;;
+    -*)
+      echo "error: unknown option: ${arg}" >&2
+      usage >&2
+      exit 1
+      ;;
+    *)
+      IFACE="${arg}"
+      ;;
+  esac
+done
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+REPO_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)"
+IGH_ROOT="${IGH_ROOT:-${REPO_ROOT}/dependency/x86/third_party/ethercat/v1.7.0}"
+ETHERCAT_CONF="${ETHERCAT_CONF:-${IGH_ROOT}/etc/ethercat.conf}"
+ETHERCATCTL="${IGH_ROOT}/sbin/ethercatctl"
+
+if [[ ! -x "${ETHERCATCTL}" ]]; then
+  echo "error: ethercatctl not found: ${ETHERCATCTL}" >&2
+  exit 1
+fi
+
+"${ETHERCATCTL}" -c "${ETHERCAT_CONF}" stop
+
+if [[ "${RESTORE_NETWORK}" == "true" ]]; then
+  ip link set "${IFACE}" up
+  if command -v nmcli >/dev/null 2>&1; then
+    nmcli device connect "${IFACE}" || true
+  fi
+  echo "Stopped EtherCAT and requested normal network restore on ${IFACE}."
+else
+  echo "Stopped EtherCAT. Normal network restore skipped."
+  echo "Use '--restore-network' if this interface should return to NetworkManager."
+fi