feat(motor): add EYOU EtherCAT CiA402 driver

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/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,88 @@
+#include <iostream>
+#include <memory>
+
+#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";
+
+class DeviceManagerDestroyGuard {
+public:
+    ~DeviceManagerDestroyGuard()
+    {
+        DeviceManager::destroyInstance();
+    }
+};
+
+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(6);
+    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();
+
+}
+
+} // namespace cmvr::device

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

@@ -0,0 +1,548 @@
+#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);
+
+    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* 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