Merge branch 'dev' into linbo_dev

# Conflicts:
#	cmvr-es/config/devices/motor/ethercat_motors.pb.txt
#	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_device_manager_real_test.cpp
#	cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp
#	dependency/x86/third_party/ethercat/v1.7.0/bin/ethercat
#	dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.a
#	dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so.1.2.0

cmake/FindExternalLib.cmake

@@ -55,7 +55,17 @@ function(setup_external_libs ARCH)
 
         # ---- library dirs ----
         if(EXISTS "${FULL_PATH}/lib")
+            file(GLOB _BUNDLED_LIBSTDCXX_FILES
+                    "${FULL_PATH}/lib/libstdc++.so"
+                    "${FULL_PATH}/lib/libstdc++.so.*"
+            )
+            if(_BUNDLED_LIBSTDCXX_FILES)
+                message(STATUS
+                        "${LIB_NAME}: excluding vendor lib directory from global "
+                        "link paths because it contains a private libstdc++")
+            else()
                 list(APPEND LIBRARY_DIRS "${FULL_PATH}/lib")
+            endif()
             set(HAS_LIB TRUE)
 
             # Collect shared libs for install: *.so and *.so.*

cmvr-es/common/CMakeLists.txt

@@ -29,6 +29,12 @@ target_link_libraries(common PUBLIC
 add_library(cmvr_es::common ALIAS common)
 install(TARGETS common LIBRARY DESTINATION lib)
 
+add_executable(support_functions_test
+        math/support_functions_test.cpp
+)
+target_include_directories(support_functions_test PRIVATE ${CMAKE_SOURCE_DIR}/cmvr-es)
+target_link_libraries(support_functions_test PRIVATE gtest gtest_main glog)
+
 #add_executable(image_display_test
 #        utils/visualization/image_display_test.cpp
 #)

cmvr-es/common/math/support_functions.h

@@ -3,6 +3,7 @@
 //
 
 #pragma once
+#include <cstdint>
 #include <cmath>
 #include <vector>
 #include <algorithm>
@@ -14,6 +15,25 @@ class SupportFunctions {
 private:
     static constexpr double EPS = 1e-9;
 public:
+    static constexpr std::int64_t absoluteDifference(const std::int32_t lhs,
+                                                     const std::int32_t rhs) noexcept {
+        return lhs >= rhs
+                   ? static_cast<std::int64_t>(lhs) - static_cast<std::int64_t>(rhs)
+                   : static_cast<std::int64_t>(rhs) - static_cast<std::int64_t>(lhs);
+    }
+
+    static constexpr std::int64_t cyclicAbsoluteDifference(
+        const std::int32_t lhs,
+        const std::int32_t rhs,
+        const std::int64_t period) noexcept {
+        const auto linear_distance = absoluteDifference(lhs, rhs);
+        if (period <= 0) {
+            return linear_distance;
+        }
+        const auto wrapped_distance = linear_distance % period;
+        return std::min(wrapped_distance, period - wrapped_distance);
+    }
+
     static  std::vector<double> eigen_to_vector(const Eigen::VectorXd &v) {
         return std::vector<double>(v.data(), v.data() + v.size());
     }

cmvr-es/common/math/support_functions_test.cpp

@@ -0,0 +1,34 @@
+#include <cstdint>
+#include <limits>
+
+#include <gtest/gtest.h>
+
+#include "common/math/support_functions.h"
+
+TEST(SupportFunctionsTest, CyclicAbsoluteDifferenceTreatsFullTurnsAsEquivalent)
+{
+    constexpr std::int64_t period = 65536LL * 101LL;
+
+    EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(5254257, -1364879, period), 0);
+    EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(-10883488, -17502624, period), 0);
+}
+
+TEST(SupportFunctionsTest, CyclicAbsoluteDifferenceUsesShortestWrappedDistance)
+{
+    constexpr std::int64_t period = 100;
+
+    EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(3, 97, period), 6);
+    EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(97, 3, period), 6);
+    EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(10, 40, period), 30);
+}
+
+TEST(SupportFunctionsTest, CyclicAbsoluteDifferenceHandlesInt32Range)
+{
+    constexpr std::int64_t period = 65536LL * 101LL;
+
+    const auto distance = SupportFunctions::cyclicAbsoluteDifference(
+        std::numeric_limits<std::int32_t>::min(),
+        std::numeric_limits<std::int32_t>::max(), period);
+    EXPECT_GE(distance, 0);
+    EXPECT_LE(distance, period / 2);
+}

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

@@ -14,15 +14,22 @@ motor {
       slave_state_poll_period_ms: 10
 
       cia402 {
-        profile_position_trigger_delay_ms: 2
-        state_transition_timeout_ms: 5000
+        state_transition_timeout_ms: 1200
         velocity_stop_timeout_ms: 2000
         status_poll_period_ms: 10
         stopped_velocity_tolerance_rad_s: 0.001
       }
 
+      zero_calibration {
+        timeout_ms: 2000
+        poll_period_ms: 10
+        stable_sample_count: 5
+        position_tolerance_counts: 10000
+        stable_delta_counts: 1000
+      }
+
       dc {
-        enable: false
+        enable: true
         reference_motor_id: 1
         sync0_cycle_us: 1000
         sync0_shift_us: 0
@@ -32,6 +39,12 @@ motor {
       }
 
       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 {
@@ -48,6 +61,12 @@ motor {
 
     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_four_real_test.pb.txt

@@ -14,13 +14,20 @@ motor {
       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
       }
 
+      zero_calibration {
+        timeout_ms: 2000
+        poll_period_ms: 10
+        stable_sample_count: 5
+        position_tolerance_counts: 10000
+        stable_delta_counts: 1000
+      }
+
       dc {
         enable: false
         reference_motor_id: 1

cmvr-es/devices/arm/aubo_arm/CMakeLists.txt

@@ -24,9 +24,34 @@ if (EXISTS "${AUBO_SDK_LIB_DIR}/cmake/aubo_sdk/aubo_sdkConfig.cmake")
         endif()
     endif()
     find_package(aubo_sdk REQUIRED CONFIG PATHS "${AUBO_SDK_LIB_DIR}/cmake/aubo_sdk" NO_DEFAULT_PATH)
+
+    # The vendor directory contains an old private libstdc++. Keep it out of
+    # consumers' RUNPATH by staging only the AUBO runtime libraries.
+    set(AUBO_CLEAN_LIB_DIR "${CMAKE_CURRENT_BINARY_DIR}/aubo_sdk_runtime")
+    file(MAKE_DIRECTORY "${AUBO_CLEAN_LIB_DIR}")
+    foreach(AUBO_LIB
+            libaubo_sdk.so
+            libaubo_sdkd.so
+            librobot_proxy.so
+            librobot_proxyd.so)
+        file(COPY_FILE
+                "${AUBO_SDK_LIB_DIR}/${AUBO_LIB}"
+                "${AUBO_CLEAN_LIB_DIR}/${AUBO_LIB}"
+                ONLY_IF_DIFFERENT
+        )
+    endforeach()
+
     set_target_properties(aubo_sdk::aubo_sdk aubo_sdk::robot_proxy PROPERTIES
             MAP_IMPORTED_CONFIG_DEBUG Release
     )
+    set_target_properties(aubo_sdk::aubo_sdk PROPERTIES
+            IMPORTED_LOCATION_RELEASE "${AUBO_CLEAN_LIB_DIR}/libaubo_sdk.so"
+            IMPORTED_LOCATION_DEBUG "${AUBO_CLEAN_LIB_DIR}/libaubo_sdkd.so"
+    )
+    set_target_properties(aubo_sdk::robot_proxy PROPERTIES
+            IMPORTED_LOCATION_RELEASE "${AUBO_CLEAN_LIB_DIR}/librobot_proxy.so"
+            IMPORTED_LOCATION_DEBUG "${AUBO_CLEAN_LIB_DIR}/librobot_proxyd.so"
+    )
     target_compile_definitions(aubo_arm PRIVATE CMVR_HAS_AUBO_SDK)
     target_include_directories(aubo_arm PRIVATE ${AUBO_SDK_INCLUDE_DIR})
     target_link_libraries(aubo_arm PRIVATE aubo_sdk::aubo_sdk)

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

@@ -1,5 +1,6 @@
 #include "arm/motor_robot_arm/include/motor_robot_arm.h"
 
+#include <algorithm>
 #include <chrono>
 #include <Eigen/Dense>
 #include <stdexcept>
@@ -309,25 +310,31 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
             return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name);
         }
         if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
-            motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
+            if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
+                return Result::failure(ArmErrorCode::CommandFailed,
+                                       "failed to set cyclic position mode for joint: " +
+                                           joint_name);
+            }
         }
         motors.push_back(std::move(motor));
     }
 
     const auto t0 = std::chrono::steady_clock::now();
     constexpr double fallback_dt = 0.001;
+    std::vector<double> command_velocity(motors.size(), 0.0);
     for (std::size_t k = 1; k < samples.size(); ++k) {
         const auto& sample = samples[k];
         if (sample.position.size() != motors.size()) {
             return Result::failure(ArmErrorCode::CommandFailed, "moveJ sample size mismatch");
         }
-        for (std::size_t i = 0; i < motors.size(); ++i) {
-            const double qd = i < sample.velocity.size() ? sample.velocity[i] : 0.0;
-            if (!motors[i]->commandCyclicPosition(sample.position[i], qd)) {
+        std::fill(command_velocity.begin(), command_velocity.end(), 0.0);
+        std::copy_n(sample.velocity.begin(),
+                    std::min(sample.velocity.size(), command_velocity.size()),
+                    command_velocity.begin());
+        if (!motor_manager_->commandCyclicPositionsAtomic(
+                motors, sample.position, command_velocity)) {
             return Result::failure(ArmErrorCode::CommandFailed,
-                                       "failed to command cyclic position for joint: " +
-                                           motors[i]->jointName());
-            }
+                                   "failed to submit atomic cyclic position command");
         }
         if (k + 1 < samples.size()) {
             const double next_t = samples[k + 1].t > 0.0 ? samples[k + 1].t
@@ -358,7 +365,11 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity,
                                        "motor not found for joint: " + joint_names_[i]);
             }
             if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) {
-                motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY);
+                if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY)) {
+                    return Result::failure(ArmErrorCode::CommandFailed,
+                                           "failed to set cyclic velocity mode for joint: " +
+                                               joint_names_[i]);
+                }
             }
             if (!motor->commandCyclicVelocity(velocity.velocity[i] * speed_scaling_)) {
                 return Result::failure(ArmErrorCode::CommandFailed,
@@ -481,6 +492,8 @@ Result MotorRobotArm::servoJ(const JointPositionCommand& target)
     }
 
     std::lock_guard<std::mutex> lock(mutex_);
+    std::vector<std::shared_ptr<AbstractMotor>> motors;
+    motors.reserve(joint_names_.size());
     for (std::size_t i = 0; i < joint_names_.size(); ++i) {
         auto motor = getMotor_(joint_names_[i]);
         if (!motor) {
@@ -488,14 +501,19 @@ Result MotorRobotArm::servoJ(const JointPositionCommand& target)
                                    "motor not found for joint: " + joint_names_[i]);
         }
         if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
-            motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
-        }
-        if (!motor->commandCyclicPosition(target.position[i], 0.0)) {
+            if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
                 return Result::failure(ArmErrorCode::CommandFailed,
-                                   "failed to command cyclic position for joint: " +
+                                       "failed to set cyclic position mode for joint: " +
                                            joint_names_[i]);
             }
         }
+        motors.push_back(std::move(motor));
+    }
+    const std::vector<double> velocities(motors.size(), 0.0);
+    if (!motor_manager_->commandCyclicPositionsAtomic(motors, target.position, velocities)) {
+        return Result::failure(ArmErrorCode::CommandFailed,
+                               "failed to submit atomic cyclic position command");
+    }
     return Result::success();
 }
 
@@ -762,7 +780,9 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj
                 return false;
             }
             if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
-                motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
+                if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
+                    return false;
+                }
             }
             motors.push_back(std::move(motor));
         }
@@ -776,11 +796,9 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj
         if (position.size() != motors.size() || velocity.size() != motors.size()) {
             return false;
         }
-        for (std::size_t j = 0; j < motors.size(); ++j) {
-            if (!motors[j]->commandCyclicPosition(position[j], velocity[j])) {
+        if (!motor_manager_->commandCyclicPositionsAtomic(motors, position, velocity)) {
             return false;
         }
-        }
         next_deadline += std::chrono::duration_cast<std::chrono::steady_clock::duration>(
             std::chrono::duration<double>(dt_segment));
         std::this_thread::sleep_until(next_deadline);

cmvr-es/devices/motor/abstract_motor.h

@@ -50,13 +50,13 @@ namespace cmvr::device{
 
         DeviceKind kind() const noexcept override { return DeviceKind::Motor; }
 
-        virtual void setMode(msgs::RunMode mode) {
+        virtual bool setMode(msgs::RunMode mode) {
             std::scoped_lock lock(mtx_);
             if (!protocol_) {
                 CMVR_LOG(ERROR) << "Protocol not set for motor";
-                return;
+                return false;
             }
-            protocol_->setMode(node_id_, mode);
+            return protocol_->setMode(node_id_, mode);
         }
 
         virtual msgs::RunMode getMode() {

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

@@ -24,6 +24,22 @@ namespace cmvr::device {
 
 class EthercatMotorBusRuntime final : public AbstractMotorBusRuntime {
 public:
+    struct PdoWrite {
+        int motor_id{0};
+        std::uint16_t index{0};
+        std::uint8_t subindex{0};
+        std::uint8_t bit_length{0};
+        std::uint64_t raw_value{0};
+    };
+
+    struct PdoRead {
+        int motor_id{0};
+        std::uint16_t index{0};
+        std::uint8_t subindex{0};
+        std::uint8_t bit_length{0};
+        std::uint64_t raw_value{0};
+    };
+
     bool init(const config::MotorGroupConfig& group_cfg) override;
     bool start() override;
     void stop() override;
@@ -45,6 +61,37 @@ public:
                             toRawValue_(value));
     }
 
+    template <typename T>
+    static PdoWrite makePdoWrite(int motor_id,
+                                 std::uint16_t index,
+                                 std::uint8_t subindex,
+                                 T value)
+    {
+        return PdoWrite{motor_id, index, subindex, valueBitLength_<T>(), toRawValue_(value)};
+    }
+
+    bool writePdosAtomic(const PdoWrite* writes, std::size_t count);
+
+    template <typename T>
+    static PdoRead makePdoRead(int motor_id,
+                               std::uint16_t index,
+                               std::uint8_t subindex)
+    {
+        return PdoRead{motor_id, index, subindex, valueBitLength_<T>(), 0};
+    }
+
+    template <typename T>
+    static T pdoReadValue(const PdoRead& read)
+    {
+        return fromRawValue_<T>(read.raw_value);
+    }
+
+    bool readPdosAtomic(PdoRead* reads, std::size_t count) const;
+
+    std::uint64_t commandGeneration() const { return command_generation_.load(); }
+    std::uint64_t sentCommandGeneration() const { return sent_command_generation_.load(); }
+    bool isHealthy() const { return healthy_.load(); }
+
     template <typename T>
     bool readPdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T& value) const
     {
@@ -94,10 +141,31 @@ private:
         std::unordered_map<std::uint32_t, PdoEntryRuntime> pdo_entries;
     };
 
+    struct BusHealthState {
+        bool initialized{false};
+        bool healthy{false};
+        unsigned int slaves_responding{0};
+        unsigned int master_al_states{0};
+        bool link_up{false};
+        int domain_result{0};
+        unsigned int working_counter{0};
+        unsigned int wc_state{0};
+    };
+
+    struct SlaveHealthState {
+        bool initialized{false};
+        bool healthy{false};
+        int result{0};
+        bool online{false};
+        bool operational{false};
+        unsigned int al_state{0};
+    };
+
     bool configureSlave_(SlaveRuntime& slave);
     bool configureDc_();
     bool waitSlavesOperational_();
     void cyclicLoop_();
+    void monitorBusHealth_();
     void readFeedbackLocked_();
     void writeCommandsLocked_();
     void releaseMaster_();
@@ -166,6 +234,12 @@ private:
     mutable std::mutex data_mutex_;
     std::thread cyclic_thread_;
     std::atomic<bool> running_{false};
+    std::atomic<bool> healthy_{false};
+    std::atomic<bool> health_monitor_enabled_{false};
+    std::atomic<std::uint64_t> command_generation_{0};
+    std::atomic<std::uint64_t> sent_command_generation_{0};
+    BusHealthState last_bus_health_;
+    std::unordered_map<int, SlaveHealthState> last_slave_health_;
     bool initialized_{false};
     bool started_{false};
 };

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

@@ -145,6 +145,10 @@ bool EthercatMotorBusRuntime::start()
     }
 
     running_.store(true);
+    healthy_.store(false);
+    health_monitor_enabled_.store(false);
+    last_bus_health_ = {};
+    last_slave_health_.clear();
     cyclic_thread_ = std::thread(&EthercatMotorBusRuntime::cyclicLoop_, this);
     started_ = true;
 
@@ -152,6 +156,7 @@ bool EthercatMotorBusRuntime::start()
         stop();
         return false;
     }
+    health_monitor_enabled_.store(true);
 
     CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] started EtherCAT runtime: " << id_;
     return true;
@@ -160,6 +165,8 @@ bool EthercatMotorBusRuntime::start()
 void EthercatMotorBusRuntime::stop()
 {
     running_.store(false);
+    healthy_.store(false);
+    health_monitor_enabled_.store(false);
     if (cyclic_thread_.joinable()) {
         cyclic_thread_.join();
     }
@@ -484,6 +491,7 @@ bool EthercatMotorBusRuntime::waitSlavesOperational_()
             last_domain_state.wc_state == EC_WC_COMPLETE;
 
         if (all_slaves_operational && domain_complete) {
+            healthy_.store(true);
             CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] all EtherCAT slaves operational: "
                            << id_
                            << ", working_counter=" << last_domain_state.working_counter;
@@ -527,6 +535,8 @@ void EthercatMotorBusRuntime::cyclicLoop_()
     bool dc_monitor_queued = false;
     auto cycle_time = std::chrono::steady_clock::now();
     auto next_dc_monitor_time = cycle_time + dc_monitor_period;
+    const auto bus_monitor_period = std::chrono::milliseconds(100);
+    auto next_bus_monitor_time = cycle_time;
     while (running_.load()) {
         if (dc_enabled) {
             ecrt_master_application_time(master_, timePointNs_(cycle_time));
@@ -551,6 +561,7 @@ void EthercatMotorBusRuntime::cyclicLoop_()
             std::lock_guard<std::mutex> lock(data_mutex_);
             readFeedbackLocked_();
             writeCommandsLocked_();
+            sent_command_generation_.store(command_generation_.load());
         }
         ecrt_domain_queue(domain_);
         if (dc_enabled) {
@@ -576,11 +587,109 @@ void EthercatMotorBusRuntime::cyclicLoop_()
         }
         ecrt_master_send(master_);
 
+        if (health_monitor_enabled_.load() && cycle_time >= next_bus_monitor_time) {
+            monitorBusHealth_();
+            do {
+                next_bus_monitor_time += bus_monitor_period;
+            } while (cycle_time >= next_bus_monitor_time);
+        }
+
         cycle_time += period;
         std::this_thread::sleep_until(cycle_time);
     }
 }
 
+void EthercatMotorBusRuntime::monitorBusHealth_()
+{
+    if (!master_ || !domain_) {
+        healthy_.store(false);
+        return;
+    }
+
+    ec_master_state_t master_state{};
+    ecrt_master_state(master_, &master_state);
+    ec_domain_state_t domain_state{};
+    const int domain_result = ecrt_domain_state(domain_, &domain_state);
+
+    bool all_slaves_healthy = true;
+    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);
+        const bool slave_healthy = result == 0 && state.online && state.operational &&
+                                   state.al_state == EC_AL_STATE_OP;
+        const SlaveHealthState current{
+            true,
+            slave_healthy,
+            result,
+            state.online != 0,
+            state.operational != 0,
+            static_cast<unsigned int>(state.al_state),
+        };
+        auto& previous = last_slave_health_[motor_id];
+        const bool changed = !previous.initialized ||
+                             previous.healthy != current.healthy ||
+                             previous.result != current.result ||
+                             previous.online != current.online ||
+                             previous.operational != current.operational ||
+                             previous.al_state != current.al_state;
+        if (changed && !current.healthy) {
+            CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] slave communication unhealthy: "
+                            << id_
+                            << ", motor_id=" << motor_id
+                            << ", result=" << current.result
+                            << ", online=" << current.online
+                            << ", operational=" << current.operational
+                            << ", al_state=" << current.al_state;
+        } else if (changed && previous.initialized && current.healthy) {
+            CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] slave communication recovered: "
+                           << id_ << ", motor_id=" << motor_id;
+        }
+        previous = current;
+        all_slaves_healthy = all_slaves_healthy && slave_healthy;
+    }
+
+    const bool domain_healthy = domain_result == 0 &&
+                                domain_state.wc_state == EC_WC_COMPLETE;
+    const bool master_healthy = master_state.link_up &&
+                                master_state.slaves_responding >= slaves_by_motor_id_.size() &&
+                                (master_state.al_states & EC_AL_STATE_OP) != 0;
+    const bool bus_healthy = master_healthy && domain_healthy && all_slaves_healthy;
+    const BusHealthState current{
+        true,
+        bus_healthy,
+        master_state.slaves_responding,
+        static_cast<unsigned int>(master_state.al_states),
+        master_state.link_up != 0,
+        domain_result,
+        domain_state.working_counter,
+        static_cast<unsigned int>(domain_state.wc_state),
+    };
+    const bool changed = !last_bus_health_.initialized ||
+                         last_bus_health_.healthy != current.healthy ||
+                         last_bus_health_.slaves_responding != current.slaves_responding ||
+                         last_bus_health_.master_al_states != current.master_al_states ||
+                         last_bus_health_.link_up != current.link_up ||
+                         last_bus_health_.domain_result != current.domain_result ||
+                         last_bus_health_.working_counter != current.working_counter ||
+                         last_bus_health_.wc_state != current.wc_state;
+    if (changed && !current.healthy) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] bus communication unhealthy: "
+                        << id_
+                        << ", link_up=" << current.link_up
+                        << ", slaves_responding=" << current.slaves_responding
+                        << ", master_al_states=" << current.master_al_states
+                        << ", domain_result=" << current.domain_result
+                        << ", wc_state=" << current.wc_state
+                        << ", working_counter=" << current.working_counter;
+    } else if (changed && last_bus_health_.initialized && current.healthy) {
+        CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] bus communication recovered: "
+                       << id_
+                       << ", working_counter=" << current.working_counter;
+    }
+    last_bus_health_ = current;
+    healthy_.store(bus_healthy);
+}
+
 void EthercatMotorBusRuntime::readFeedbackLocked_()
 {
     if (!domain_data_) {
@@ -642,23 +751,51 @@ bool EthercatMotorBusRuntime::writePdoRaw_(const int motor_id,
                                            const std::uint8_t bit_len,
                                            const std::uint64_t value)
 {
+    const PdoWrite write{motor_id, index, subindex, bit_len, value};
+    return writePdosAtomic(&write, 1);
+}
+
+bool EthercatMotorBusRuntime::writePdosAtomic(const PdoWrite* writes,
+                                               const std::size_t count)
+{
+    if (!writes || count == 0) {
+        return false;
+    }
+
     std::lock_guard<std::mutex> lock(data_mutex_);
-    auto slave_it = slaves_by_motor_id_.find(motor_id);
+
+    for (std::size_t i = 0; i < count; ++i) {
+        const auto& write = writes[i];
+        if (!isSupportedBitLength_(write.bit_length)) {
+            return false;
+        }
+        const auto slave_it = slaves_by_motor_id_.find(write.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()) {
+        const auto entry_it = slave_it->second.pdo_entries.find(
+            pdoEntryKey_(write.index, write.subindex));
+        if (entry_it == slave_it->second.pdo_entries.end() ||
+            !entry_it->second.rx ||
+            entry_it->second.cfg.bit_len != write.bit_length) {
             return false;
         }
-    auto& entry = entry_it->second;
-    if (!entry.rx || entry.cfg.bit_len != bit_len) {
+        for (std::size_t previous = 0; previous < i; ++previous) {
+            if (writes[previous].motor_id == write.motor_id &&
+                writes[previous].index == write.index &&
+                writes[previous].subindex == write.subindex) {
                 return false;
             }
-    entry.value = maskValue_(value, entry.cfg.bit_len);
-    if (domain_data_) {
-        writeEntryValue_(domain_data_, entry);
         }
+    }
+
+    for (std::size_t i = 0; i < count; ++i) {
+        const auto& write = writes[i];
+        auto& entry = slaves_by_motor_id_.at(write.motor_id)
+                          .pdo_entries.at(pdoEntryKey_(write.index, write.subindex));
+        entry.value = maskValue_(write.raw_value, entry.cfg.bit_len);
+    }
+    command_generation_.fetch_add(1);
     return true;
 }
 
@@ -668,20 +805,46 @@ bool EthercatMotorBusRuntime::readPdoRaw_(const int motor_id,
                                           const std::uint8_t bit_len,
                                           std::uint64_t& value) const
 {
+    PdoRead read{motor_id, index, subindex, bit_len, 0};
+    if (!readPdosAtomic(&read, 1)) {
+        return false;
+    }
+    value = read.raw_value;
+    return true;
+}
+
+bool EthercatMotorBusRuntime::readPdosAtomic(PdoRead* reads,
+                                              const std::size_t count) const
+{
+    if (!reads || count == 0) {
+        return false;
+    }
+
     std::lock_guard<std::mutex> lock(data_mutex_);
-    const auto slave_it = slaves_by_motor_id_.find(motor_id);
+    for (std::size_t i = 0; i < count; ++i) {
+        const auto& read = reads[i];
+        if (!isSupportedBitLength_(read.bit_length)) {
+            return false;
+        }
+        const auto slave_it = slaves_by_motor_id_.find(read.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()) {
+        const auto entry_it = slave_it->second.pdo_entries.find(
+            pdoEntryKey_(read.index, read.subindex));
+        if (entry_it == slave_it->second.pdo_entries.end() ||
+            entry_it->second.rx ||
+            entry_it->second.cfg.bit_len != read.bit_length) {
             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);
+
+    for (std::size_t i = 0; i < count; ++i) {
+        auto& read = reads[i];
+        const auto& entry = slaves_by_motor_id_.at(read.motor_id)
+                                .pdo_entries.at(pdoEntryKey_(read.index, read.subindex));
+        read.raw_value = maskValue_(entry.value, entry.cfg.bit_len);
+    }
     return true;
 }
 

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

@@ -1,6 +1,7 @@
 #include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
 
 #include <algorithm>
+#include <array>
 #include <chrono>
 #include <cstdint>
 #include <iostream>
@@ -74,17 +75,42 @@ TEST(EthercatMotorBusRuntimeRealTest, InitStartAndReadStatusword)
         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 std::array command_writes{
+        EthercatMotorBusRuntime::makePdoWrite<std::uint16_t>(
+            1, msgs::CIA402_CONTROL_WORD_6040, 0x00, 0x0000),
+        EthercatMotorBusRuntime::makePdoWrite<std::int8_t>(
+            1, msgs::CIA402_OPERATION_MODE_6060, 0x00, 0),
+    };
+    auto invalid_writes = command_writes;
+    invalid_writes[1].bit_length = 16;
+
+    const auto generation_before = runtime.commandGeneration();
+    EXPECT_FALSE(runtime.writePdosAtomic(invalid_writes.data(), invalid_writes.size()));
+    EXPECT_EQ(runtime.commandGeneration(), generation_before);
+    EXPECT_TRUE(runtime.writePdosAtomic(command_writes.data(), command_writes.size()));
+    EXPECT_EQ(runtime.commandGeneration(), generation_before + 1);
 
     const int settle_ms = 1000;
     std::this_thread::sleep_for(std::chrono::milliseconds(settle_ms));
+    EXPECT_GE(runtime.sentCommandGeneration(), generation_before + 1);
+    EXPECT_TRUE(runtime.isHealthy());
 
-    std::uint16_t statusword = 0;
-    EXPECT_TRUE(runtime.readPdo<std::uint16_t>(1, msgs::CIA402_STATUS_WORD_6041, 0x00, statusword));
+    std::array feedback_reads{
+        EthercatMotorBusRuntime::makePdoRead<std::uint16_t>(
+            1, msgs::CIA402_STATUS_WORD_6041, 0x00),
+        EthercatMotorBusRuntime::makePdoRead<std::int8_t>(
+            1, msgs::CIA402_MODE_DISPLAY_6061, 0x00),
+    };
+    auto invalid_feedback_reads = feedback_reads;
+    invalid_feedback_reads[1].bit_length = 16;
+    EXPECT_FALSE(runtime.readPdosAtomic(invalid_feedback_reads.data(),
+                                        invalid_feedback_reads.size()));
+    ASSERT_TRUE(runtime.readPdosAtomic(feedback_reads.data(), feedback_reads.size()));
 
-    std::int8_t mode_display = 0;
-    EXPECT_TRUE(runtime.readPdo<std::int8_t>(1, msgs::CIA402_MODE_DISPLAY_6061, 0x00, mode_display));
+    const auto statusword =
+        EthercatMotorBusRuntime::pdoReadValue<std::uint16_t>(feedback_reads[0]);
+    const auto mode_display =
+        EthercatMotorBusRuntime::pdoReadValue<std::int8_t>(feedback_reads[1]);
 
     std::int16_t actual_current = 0;
     EXPECT_TRUE(runtime.readPdo<std::int16_t>(

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

@@ -1,5 +1,6 @@
 add_library(ethercat_motor_driver SHARED
         src/cia402/cia402_protocol.cpp
+        src/cia402/cia402_status_monitor.cpp
         src/vendor/eyou/eyou_motor.cpp
         src/vendor/eyou/eyou_motor_adapter.cpp
 )

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

@@ -156,6 +156,11 @@ inline bool targetReached(const Statusword status)
     return status.target_reached != 0;
 }
 
+inline bool setPointAcknowledged(const Statusword status)
+{
+    return (status.value & (1U << 12U)) != 0;
+}
+
 } // namespace cmvr::device::cia402
 
 #endif // CMVR_ES_CIA402_OBJECTS_H

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

@@ -3,7 +3,9 @@
 
 #include <cstdint>
 #include <memory>
+#include <mutex>
 #include <unordered_map>
+#include <vector>
 
 #include "cmvr/config/motor_config/motor_config.pb.h"
 #include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
@@ -12,15 +14,29 @@
 
 namespace cmvr::device {
 
+class Cia402StatusMonitor;
+
 class Cia402Protocol final : public MotorProtocolInterface {
 public:
+    struct CyclicPositionCommand {
+        std::uint8_t node_id{0};
+        double target_q{0.0};
+        double target_qd{0.0};
+    };
+
+    struct MotorFeedback {
+        std::uint8_t node_id{0};
+        double q{0.0};
+        double qd{0.0};
+    };
+
     explicit Cia402Protocol(std::shared_ptr<EthercatMotorBusRuntime> bus_runtime,
                             const config::Cia402ProtocolConfig& config);
-    ~Cia402Protocol() override = default;
+    ~Cia402Protocol() override;
 
     bool initNode(std::uint8_t node_id) override;
 
-    void setMode(std::uint8_t node_id, msgs::RunMode mode) override;
+    bool 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;
@@ -37,6 +53,9 @@ public:
     bool commandCyclicPosition(std::uint8_t node_id,
                                double target_q,
                                double target_qd) override;
+    bool commandCyclicPositionsAtomic(const CyclicPositionCommand* commands,
+                                      std::size_t count);
+    bool readFeedbacksAtomic(MotorFeedback* feedbacks, std::size_t count) const;
     bool commandCyclicVelocity(std::uint8_t node_id,
                                double target_qd) override;
     bool commandCyclicTorque(std::uint8_t node_id, double target_tau) override;
@@ -97,18 +116,32 @@ private:
     bool waitStatus_(std::uint8_t node_id,
                      cia402::DeviceState target_state,
                      const char* state_name) const;
+    bool waitMode_(std::uint8_t node_id, std::int8_t target_mode) const;
+    bool waitSetPointAcknowledged_(std::uint8_t node_id, bool acknowledged) 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);
+    bool writeTargetsForMode_(std::uint8_t node_id,
+                              msgs::RunMode mode,
+                              const NodeState& state) const;
+    bool appendTargetWritesForMode_(
+        std::uint8_t node_id,
+        msgs::RunMode mode,
+        const NodeState& state,
+        EthercatMotorBusRuntime::PdoWrite* writes,
+        std::size_t capacity,
+        std::size_t& count) const;
+    bool writeProfilePositionTarget_(std::uint8_t node_id, NodeState& state);
+    bool writeNode_(std::uint8_t node_id, NodeState& state);
 
     std::shared_ptr<EthercatMotorBusRuntime> bus_runtime_;
+    std::unique_ptr<Cia402StatusMonitor> status_monitor_;
     config::Cia402ProtocolConfig config_;
     std::unordered_map<std::uint8_t, NodeState> nodes_;
+    std::mutex cyclic_position_mutex_;
+    std::vector<EthercatMotorBusRuntime::PdoWrite> cyclic_position_writes_;
 };
 
 } // namespace cmvr::device

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

@@ -0,0 +1,78 @@
+#ifndef CMVR_ES_CIA402_STATUS_MONITOR_H
+#define CMVR_ES_CIA402_STATUS_MONITOR_H
+
+#include <atomic>
+#include <chrono>
+#include <cstdint>
+#include <memory>
+#include <mutex>
+#include <thread>
+#include <unordered_map>
+
+#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
+
+namespace cmvr::device {
+
+class Cia402StatusMonitor final {
+public:
+    Cia402StatusMonitor(std::shared_ptr<EthercatMotorBusRuntime> bus_runtime,
+                        std::chrono::milliseconds poll_period);
+    ~Cia402StatusMonitor();
+
+    Cia402StatusMonitor(const Cia402StatusMonitor&) = delete;
+    Cia402StatusMonitor& operator=(const Cia402StatusMonitor&) = delete;
+
+    void addNode(std::uint8_t node_id);
+    void setExpectedOperationEnabled(std::uint8_t node_id, bool expected);
+    bool isNodeOperational(std::uint8_t node_id) const;
+
+private:
+    struct StatusSample {
+        bool read_ok{false};
+        bool transport_healthy{false};
+        bool expected_operation_enabled{false};
+        bool operation_enabled{false};
+        bool status_problem{false};
+        bool command_blocked{false};
+        std::uint16_t statusword{0};
+        std::uint16_t error_code{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};
+    };
+
+    struct NodeMonitorState {
+        bool expected_operation_enabled{false};
+        bool has_last_sample{false};
+        StatusSample last_sample;
+    };
+
+    void monitorLoop_();
+    void monitorNode_(std::uint8_t node_id, bool expected_operation_enabled);
+    bool readStatusSample_(std::uint8_t node_id,
+                           bool expected_operation_enabled,
+                           StatusSample& sample) const;
+    void reportErrorCodeTransition_(std::uint8_t node_id,
+                                    bool had_previous,
+                                    const StatusSample& previous,
+                                    const StatusSample& current) const;
+    void reportStatuswordTransition_(std::uint8_t node_id,
+                                     bool had_previous,
+                                     const StatusSample& previous,
+                                     const StatusSample& current) const;
+    static const char* deviceStateName_(std::uint16_t statusword);
+    static const char* errorCodeDescription_(std::uint16_t error_code);
+
+    std::shared_ptr<EthercatMotorBusRuntime> bus_runtime_;
+    std::chrono::milliseconds poll_period_;
+    std::mutex monitor_mutex_;
+    mutable std::mutex states_mutex_;
+    std::unordered_map<std::uint8_t, NodeMonitorState> states_;
+    std::atomic<bool> running_{true};
+    std::thread monitor_thread_;
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_CIA402_STATUS_MONITOR_H

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

@@ -3,6 +3,7 @@
 
 #include <cstdint>
 #include <memory>
+#include <vector>
 
 #include "cmvr/config/motor_config/motor_config.pb.h"
 #include "devices/motor/abstract_motor.h"
@@ -25,6 +26,15 @@ public:
     bool brakeRelease() override;
     std::int16_t getCurrent() override;
 
+    static bool commandCyclicPositionsAtomic(
+        const std::vector<std::shared_ptr<AbstractMotor>>& motors,
+        const std::vector<double>& positions,
+        const std::vector<double>& velocities);
+    static bool readFeedbacksAtomic(
+        const std::vector<std::shared_ptr<AbstractMotor>>& motors,
+        std::vector<double>& positions,
+        std::vector<double>& velocities);
+
 private:
     bool hasDependencies_() const;
     bool hasValidConversion_() const;

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

@@ -22,6 +22,7 @@ public:
     bool writeVelocityLimit(std::uint8_t node_id,
                             std::uint32_t velocity_limit) override;
     bool calibrateZero(std::uint8_t node_id,
+                       std::int64_t counts_per_joint_revolution,
                        std::int32_t& zeroed_position) override;
     bool brakeRelease(std::uint8_t node_id) override;
     bool readActualCurrent(std::uint8_t node_id,

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

@@ -16,6 +16,7 @@ public:
     virtual bool writeVelocityLimit(std::uint8_t node_id,
                                     std::uint32_t velocity_limit) = 0;
     virtual bool calibrateZero(std::uint8_t node_id,
+                               std::int64_t counts_per_joint_revolution,
                                std::int32_t& zeroed_position) = 0;
     virtual bool brakeRelease(std::uint8_t node_id) = 0;
 };

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

@@ -1,6 +1,7 @@
 #include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h"
 
 #include <algorithm>
+#include <array>
 #include <chrono>
 #include <cmath>
 #include <thread>
@@ -9,6 +10,7 @@
 #include "common/base/logging/logger.h"
 #include "cmvr/msgs/cia402.pb.h"
 #include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h"
+#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_status_monitor.h"
 
 namespace cmvr::device {
 
@@ -18,8 +20,13 @@ Cia402Protocol::Cia402Protocol(std::shared_ptr<EthercatMotorBusRuntime> bus_runt
       config_(config)
 {
     comm_proto = CommProto::ETHERCAT;
+    cyclic_position_writes_.reserve(512);
+    status_monitor_ = std::make_unique<Cia402StatusMonitor>(
+        bus_runtime_, std::chrono::milliseconds(config_.status_poll_period_ms()));
 }
 
+Cia402Protocol::~Cia402Protocol() = default;
+
 bool Cia402Protocol::initNode(const std::uint8_t node_id)
 {
     if (!bus_runtime_ || !bus_runtime_->hasMotor(node_id)) {
@@ -36,7 +43,10 @@ bool Cia402Protocol::initNode(const std::uint8_t node_id)
     if (readActualPosition_(node_id, actual_position)) {
         state.target_position = actual_position;
     }
-    writeNode_(node_id, state);
+    if (!writeNode_(node_id, state)) {
+        return false;
+    }
+    status_monitor_->addNode(node_id);
     return true;
 }
 
@@ -46,7 +56,8 @@ bool Cia402Protocol::commandProfilePosition(const std::uint8_t node_id,
                                                     const double max_qdd)
 {
     auto& state = nodeState_(node_id);
-    if (!hasValidConversion_(node_id, state)) {
+    if (!hasValidConversion_(node_id, state) ||
+        !status_monitor_->isNodeOperational(node_id)) {
         return false;
     }
     state.target_position = radToCounts_(target_q, state);
@@ -56,8 +67,7 @@ bool Cia402Protocol::commandProfilePosition(const std::uint8_t node_id,
         state.profile_acceleration = profile_acceleration;
         state.profile_deceleration = profile_acceleration;
     }
-    writeProfilePositionTarget_(node_id, state);
-    return true;
+    return writeProfilePositionTarget_(node_id, state);
 }
 
 bool Cia402Protocol::commandProfileVelocity(const std::uint8_t node_id,
@@ -65,7 +75,8 @@ bool Cia402Protocol::commandProfileVelocity(const std::uint8_t node_id,
                                                     const double max_qdd)
 {
     auto& state = nodeState_(node_id);
-    if (!hasValidConversion_(node_id, state)) {
+    if (!hasValidConversion_(node_id, state) ||
+        !status_monitor_->isNodeOperational(node_id)) {
         return false;
     }
     state.target_velocity = radPerSecToCounts_(target_qd, state);
@@ -74,21 +85,109 @@ bool Cia402Protocol::commandProfileVelocity(const std::uint8_t node_id,
         state.profile_acceleration = profile_acceleration;
         state.profile_deceleration = profile_acceleration;
     }
-    writeNode_(node_id, state);
-    return true;
+    return writeNode_(node_id, state);
 }
 
 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)) {
+    const CyclicPositionCommand command{node_id, target_q, target_qd};
+    return commandCyclicPositionsAtomic(&command, 1);
+}
+
+bool Cia402Protocol::commandCyclicPositionsAtomic(
+    const CyclicPositionCommand* commands,
+    const std::size_t count)
+{
+    if (!bus_runtime_ || !commands || count == 0 || count > 256) {
         return false;
     }
-    state.target_position = radToCounts_(target_q, state);
-    state.target_velocity = radPerSecToCounts_(target_qd, state);
-    writeNode_(node_id, state);
+
+    std::lock_guard<std::mutex> lock(cyclic_position_mutex_);
+    cyclic_position_writes_.clear();
+    cyclic_position_writes_.reserve(count * 2);
+
+    for (std::size_t i = 0; i < count; ++i) {
+        const auto& command = commands[i];
+        const auto* state = findNodeState_(command.node_id);
+        if (!state || !hasValidConversion_(command.node_id, *state) ||
+            state->mode != msgs::RUN_MODE_CYCLIC_SYNC_POSITION ||
+            !status_monitor_->isNodeOperational(command.node_id)) {
+            return false;
+        }
+        for (std::size_t previous = 0; previous < i; ++previous) {
+            if (commands[previous].node_id == command.node_id) {
+                return false;
+            }
+        }
+
+        cyclic_position_writes_.push_back(
+            EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
+                command.node_id,
+                msgs::CIA402_TARGET_POSITION_607A,
+                0x00,
+                radToCounts_(command.target_q, *state)));
+        cyclic_position_writes_.push_back(
+            EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
+                command.node_id,
+                msgs::CIA402_TARGET_VELOCITY_60FF,
+                0x00,
+                radPerSecToCounts_(command.target_qd, *state)));
+    }
+
+    if (!bus_runtime_->writePdosAtomic(cyclic_position_writes_.data(),
+                                       cyclic_position_writes_.size())) {
+        return false;
+    }
+
+    for (std::size_t i = 0; i < count; ++i) {
+        auto& state = nodeState_(commands[i].node_id);
+        state.target_position = radToCounts_(commands[i].target_q, state);
+        state.target_velocity = radPerSecToCounts_(commands[i].target_qd, state);
+    }
+    return true;
+}
+
+bool Cia402Protocol::readFeedbacksAtomic(MotorFeedback* feedbacks,
+                                         const std::size_t count) const
+{
+    if (!bus_runtime_ || !feedbacks || count == 0 || count > 256) {
+        return false;
+    }
+
+    std::array<EthercatMotorBusRuntime::PdoRead, 512> reads{};
+    for (std::size_t i = 0; i < count; ++i) {
+        const auto* state = findNodeState_(feedbacks[i].node_id);
+        if (!state || !hasValidConversion_(feedbacks[i].node_id, *state)) {
+            return false;
+        }
+        for (std::size_t previous = 0; previous < i; ++previous) {
+            if (feedbacks[previous].node_id == feedbacks[i].node_id) {
+                return false;
+            }
+        }
+        reads[2 * i] = EthercatMotorBusRuntime::makePdoRead<std::int32_t>(
+            feedbacks[i].node_id, msgs::CIA402_ACTUAL_POSITION_6064, 0x00);
+        reads[2 * i + 1] = EthercatMotorBusRuntime::makePdoRead<std::int32_t>(
+            feedbacks[i].node_id, msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00);
+    }
+
+    if (!bus_runtime_->readPdosAtomic(reads.data(), count * 2)) {
+        return false;
+    }
+
+    for (std::size_t i = 0; i < count; ++i) {
+        const auto* state = findNodeState_(feedbacks[i].node_id);
+        if (!state) {
+            return false;
+        }
+        const auto position = EthercatMotorBusRuntime::pdoReadValue<std::int32_t>(reads[2 * i]);
+        const auto velocity =
+            EthercatMotorBusRuntime::pdoReadValue<std::int32_t>(reads[2 * i + 1]);
+        feedbacks[i].q = countsToRad_(position, *state);
+        feedbacks[i].qd = countsToRadPerSec_(velocity, *state);
+    }
     return true;
 }
 
@@ -96,12 +195,12 @@ bool Cia402Protocol::commandCyclicVelocity(const std::uint8_t node_id,
                                                    const double target_qd)
 {
     auto& state = nodeState_(node_id);
-    if (!hasValidConversion_(node_id, state)) {
+    if (!hasValidConversion_(node_id, state) ||
+        !status_monitor_->isNodeOperational(node_id)) {
         return false;
     }
     state.target_velocity = radPerSecToCounts_(target_qd, state);
-    writeNode_(node_id, state);
-    return true;
+    return writeTargetsForMode_(node_id, state.mode, state);
 }
 
 bool Cia402Protocol::commandCyclicTorque(const std::uint8_t node_id,
@@ -113,15 +212,20 @@ bool Cia402Protocol::commandCyclicTorque(const std::uint8_t node_id,
     return false;
 }
 
-void Cia402Protocol::setMode(const std::uint8_t node_id, const msgs::RunMode mode)
+bool Cia402Protocol::setMode(const std::uint8_t node_id, const msgs::RunMode mode)
 {
     auto& state = nodeState_(node_id);
     if (!prepareSafeTargetsForMode_(node_id, mode, state)) {
-        return;
+        return false;
     }
-    writeTargetsForMode_(node_id, mode, state);
     state.mode = mode;
-    writeNode_(node_id, state);
+    if (!writeNode_(node_id, state)) {
+        CMVR_LOG(ERROR) << "[Cia402Protocol] failed to write mode/controlword PDOs, node="
+                        << static_cast<int>(node_id)
+                        << ", mode=" << static_cast<int>(mode);
+        return false;
+    }
+    return waitMode_(node_id, toCia402Mode_(mode));
 }
 
 msgs::RunMode Cia402Protocol::getMode(const std::uint8_t node_id)
@@ -201,6 +305,7 @@ bool Cia402Protocol::torqueOn(const std::uint8_t node_id)
         return false;
     }
 
+    status_monitor_->setExpectedOperationEnabled(node_id, false);
     auto& state = nodeState_(node_id);
     std::uint16_t statusword = 0;
     if (readStatusword_(node_id, statusword) && cia402::statusword(statusword).fault != 0) {
@@ -217,7 +322,9 @@ bool Cia402Protocol::torqueOn(const std::uint8_t node_id)
     state.mode = msgs::RUN_MODE_PROFILE_POSITION;
     state.target_velocity = 0;
     state.target_torque = 0;
-    writeTargetsForMode_(node_id, state.mode, state);
+    if (!writeTargetsForMode_(node_id, state.mode, state)) {
+        return false;
+    }
     if (!bus_runtime_->writePdo<std::int8_t>(node_id, msgs::CIA402_OPERATION_MODE_6060, 0x00,
                                   toCia402Mode_(state.mode))) {
         return false;
@@ -238,22 +345,22 @@ bool Cia402Protocol::torqueOn(const std::uint8_t node_id)
                                   "Operation Enabled")) {
         return false;
     }
+    if (!waitMode_(node_id, toCia402Mode_(state.mode))) {
+        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);
+    const bool enabled = writeTargetsForMode_(node_id, state.mode, state) &&
+                         writeControlword_(node_id, state.controlword) &&
+                         waitSetPointAcknowledged_(node_id, false);
+    if (enabled) {
+        status_monitor_->setExpectedOperationEnabled(node_id, true);
+    }
+    return enabled;
 }
 
 bool Cia402Protocol::torqueOff(const std::uint8_t node_id)
@@ -262,6 +369,7 @@ bool Cia402Protocol::torqueOff(const std::uint8_t node_id)
         return false;
     }
 
+    status_monitor_->setExpectedOperationEnabled(node_id, false);
     auto& state = nodeState_(node_id);
     std::int32_t actual_position = 0;
     if (readActualPosition_(node_id, actual_position)) {
@@ -269,21 +377,41 @@ bool Cia402Protocol::torqueOff(const std::uint8_t node_id)
     }
     state.target_velocity = 0;
     state.target_torque = 0;
-    writeTargetsForMode_(node_id, state.mode, state);
+    if (!writeTargetsForMode_(node_id, state.mode, state)) {
+        return false;
+    }
     waitVelocityNearZero_(node_id, "torqueOff");
 
+    std::uint16_t statusword = 0;
+    if (!readStatusword_(node_id, statusword)) {
+        return false;
+    }
+    const auto status = cia402::statusword(statusword);
+    if (cia402::hasState(status, cia402::DeviceState::SwitchOnDisabled) ||
+        cia402::hasState(status, cia402::DeviceState::ReadyToSwitchOn)) {
+        return true;
+    }
+
+    if (cia402::hasState(status, cia402::DeviceState::OperationEnabled)) {
         if (!writeControlwordAndWait_(node_id, cia402::switchOnControlword(),
                                       cia402::DeviceState::SwitchedOn,
                                       "Switched On")) {
             return false;
         }
-    if (!writeControlwordAndWait_(node_id, cia402::shutdownControlword(),
+        return writeControlwordAndWait_(node_id, cia402::shutdownControlword(),
                                         cia402::DeviceState::ReadyToSwitchOn,
-                                  "Ready To Switch On")) {
-        return false;
+                                        "Ready To Switch On");
     }
 
-    return true;
+    if (cia402::hasState(status, cia402::DeviceState::SwitchedOn)) {
+        return writeControlwordAndWait_(node_id, cia402::shutdownControlword(),
+                                        cia402::DeviceState::ReadyToSwitchOn,
+                                        "Ready To Switch On");
+    }
+
+    return writeControlwordAndWait_(node_id, cia402::controlword(0),
+                                    cia402::DeviceState::SwitchOnDisabled,
+                                    "Switch On Disabled");
 }
 
 bool Cia402Protocol::brakeRelease(const std::uint8_t node_id)
@@ -299,10 +427,13 @@ bool Cia402Protocol::quickStop(const std::uint8_t node_id)
         return false;
     }
 
+    status_monitor_->setExpectedOperationEnabled(node_id, false);
     auto& state = nodeState_(node_id);
     state.target_velocity = 0;
     state.target_torque = 0;
-    writeTargetsForMode_(node_id, state.mode, state);
+    if (!writeTargetsForMode_(node_id, state.mode, state)) {
+        return false;
+    }
 
     state.controlword = cia402::quickStopControlword();
     if (!writeControlword_(node_id, state.controlword)) {
@@ -350,8 +481,7 @@ bool Cia402Protocol::syncTargetToActualPosition(const std::uint8_t node_id)
     state.target_position = actual_position;
     state.target_velocity = 0;
     state.target_torque = 0;
-    writeTargetsForMode_(node_id, state.mode, state);
-    return true;
+    return writeTargetsForMode_(node_id, state.mode, state);
 }
 
 std::int8_t Cia402Protocol::toCia402Mode_(const msgs::RunMode mode)
@@ -611,6 +741,50 @@ bool Cia402Protocol::waitStatus_(const std::uint8_t node_id,
     return false;
 }
 
+bool Cia402Protocol::waitMode_(const std::uint8_t node_id,
+                               const std::int8_t target_mode) const
+{
+    const auto deadline = std::chrono::steady_clock::now() +
+                          std::chrono::milliseconds(config_.state_transition_timeout_ms());
+    std::int8_t last_mode = 0;
+    do {
+        if (readModeDisplay_(node_id, last_mode) && last_mode == target_mode) {
+            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 operation mode, node="
+                    << static_cast<int>(node_id)
+                    << ", target_mode=" << static_cast<int>(target_mode)
+                    << ", last_mode=" << static_cast<int>(last_mode);
+    return false;
+}
+
+bool Cia402Protocol::waitSetPointAcknowledged_(
+    const std::uint8_t node_id,
+    const bool acknowledged) 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::setPointAcknowledged(cia402::statusword(last_statusword)) == acknowledged) {
+            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 set-point acknowledge="
+                    << acknowledged
+                    << ", 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
 {
@@ -720,11 +894,11 @@ bool Cia402Protocol::writeAccelerationLimitsToDictionary_(
     return ok;
 }
 
-void Cia402Protocol::writeProfilePositionTarget_(const std::uint8_t node_id,
+bool Cia402Protocol::writeProfilePositionTarget_(const std::uint8_t node_id,
                                                   NodeState& state)
 {
     if (!bus_runtime_) {
-        return;
+        return false;
     }
 
     if (state.profile_velocity <= 0 && state.limit_qd > 0.0) {
@@ -736,16 +910,27 @@ void Cia402Protocol::writeProfilePositionTarget_(const std::uint8_t node_id,
         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()));
+    if (!writeTargetsForMode_(node_id, state.mode, state)) {
+        return false;
+    }
 
+    state.controlword = cia402::profilePositionControlword(false);
+    if (!writeControlword_(node_id, state.controlword) ||
+        !waitSetPointAcknowledged_(node_id, false)) {
+        return false;
+    }
+
+    state.controlword = cia402::profilePositionControlword(true);
+    if (!writeControlword_(node_id, state.controlword) ||
+        !waitSetPointAcknowledged_(node_id, true)) {
         state.controlword = cia402::profilePositionControlword(false);
         writeControlword_(node_id, state.controlword);
+        return false;
+    }
+
+    state.controlword = cia402::profilePositionControlword(false);
+    return writeControlword_(node_id, state.controlword) &&
+           waitSetPointAcknowledged_(node_id, false);
 }
 
 bool Cia402Protocol::prepareSafeTargetsForMode_(const std::uint8_t node_id,
@@ -782,71 +967,132 @@ bool Cia402Protocol::prepareSafeTargetsForMode_(const std::uint8_t node_id,
     return true;
 }
 
-void Cia402Protocol::writeTargetsForMode_(const std::uint8_t node_id,
+bool Cia402Protocol::writeTargetsForMode_(const std::uint8_t node_id,
                                           const msgs::RunMode mode,
                                           const NodeState& state) const
 {
     if (!bus_runtime_) {
-        return;
+        return false;
     }
 
+    std::array<EthercatMotorBusRuntime::PdoWrite, 4> writes{};
+    std::size_t count = 0;
+    if (!appendTargetWritesForMode_(node_id, mode, state,
+                                    writes.data(), writes.size(), count)) {
+        return false;
+    }
+    return count == 0 || bus_runtime_->writePdosAtomic(writes.data(), count);
+}
+
+bool Cia402Protocol::appendTargetWritesForMode_(
+    const std::uint8_t node_id,
+    const msgs::RunMode mode,
+    const NodeState& state,
+    EthercatMotorBusRuntime::PdoWrite* writes,
+    const std::size_t capacity,
+    std::size_t& count) const
+{
+    if (!bus_runtime_ || !writes) {
+        return false;
+    }
+
+    const auto append = [&](const auto write) {
+        if (count >= capacity) {
+            return false;
+        }
+        writes[count++] = write;
+        return true;
+    };
+
     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 (!append(EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
+                    node_id, msgs::CIA402_TARGET_POSITION_607A,
+                    0x00, state.target_position))) {
+                return false;
+            }
             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 (!append(EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
+                        node_id, msgs::CIA402_PROFILE_VELOCITY_6081,
+                        0x00, state.profile_velocity))) {
+                    return false;
+                }
             }
             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 (!append(EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
+                        node_id, msgs::CIA402_PROFILE_ACCELERATION_6083,
+                        0x00, state.profile_acceleration))) {
+                    return false;
+                }
             }
             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);
+                if (!append(EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
+                        node_id, msgs::CIA402_PROFILE_DECELERATION_6084,
+                        0x00, state.profile_deceleration))) {
+                    return false;
+                }
             }
             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 (!append(EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
+                    node_id, msgs::CIA402_TARGET_VELOCITY_60FF,
+                    0x00, state.target_velocity))) {
+                return false;
+            }
             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 (!append(EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
+                        node_id, msgs::CIA402_PROFILE_ACCELERATION_6083,
+                        0x00, state.profile_acceleration))) {
+                    return false;
+                }
             }
             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);
+                if (!append(EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
+                        node_id, msgs::CIA402_PROFILE_DECELERATION_6084,
+                        0x00, state.profile_deceleration))) {
+                    return false;
+                }
             }
             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);
+            if (!append(EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
+                    node_id, msgs::CIA402_TARGET_POSITION_607A,
+                    0x00, state.target_position)) ||
+                !append(EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
+                    node_id, msgs::CIA402_TARGET_VELOCITY_60FF,
+                    0x00, state.target_velocity))) {
+                return false;
+            }
             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);
+            if (!append(EthercatMotorBusRuntime::makePdoWrite<std::int32_t>(
+                    node_id, msgs::CIA402_TARGET_VELOCITY_60FF,
+                    0x00, state.target_velocity))) {
+                return false;
+            }
             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);
+            if (!append(EthercatMotorBusRuntime::makePdoWrite<std::int16_t>(
+                    node_id, msgs::CIA402_TARGET_TORQUE_6071,
+                    0x00, state.target_torque))) {
+                return false;
+            }
             break;
 
         default:
             break;
     }
+    return true;
 }
 
-void Cia402Protocol::writeNode_(const std::uint8_t node_id, NodeState& state)
+bool Cia402Protocol::writeNode_(const std::uint8_t node_id, NodeState& state)
 {
     if (!bus_runtime_) {
-        return;
+        return false;
     }
 
     std::uint16_t statusword = 0;
@@ -861,12 +1107,17 @@ void Cia402Protocol::writeNode_(const std::uint8_t node_id, NodeState& state)
         }
     }
 
-    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);
+    std::array<EthercatMotorBusRuntime::PdoWrite, 6> writes{};
+    std::size_t count = 0;
+    writes[count++] = EthercatMotorBusRuntime::makePdoWrite<std::uint16_t>(
+        node_id, msgs::CIA402_CONTROL_WORD_6040, 0x00, state.controlword.value);
+    writes[count++] = EthercatMotorBusRuntime::makePdoWrite<std::int8_t>(
+        node_id, msgs::CIA402_OPERATION_MODE_6060, 0x00, toCia402Mode_(state.mode));
+    if (!appendTargetWritesForMode_(node_id, state.mode, state,
+                                    writes.data(), writes.size(), count)) {
+        return false;
+    }
+    return bus_runtime_->writePdosAtomic(writes.data(), count);
 }
 
 } // namespace cmvr::device

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

@@ -0,0 +1,284 @@
+#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_status_monitor.h"
+
+#include <algorithm>
+#include <array>
+#include <iomanip>
+#include <utility>
+
+#include "cmvr/msgs/cia402.pb.h"
+#include "common/base/logging/logger.h"
+#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h"
+
+namespace cmvr::device {
+
+Cia402StatusMonitor::Cia402StatusMonitor(
+    std::shared_ptr<EthercatMotorBusRuntime> bus_runtime,
+    const std::chrono::milliseconds poll_period)
+    : bus_runtime_(std::move(bus_runtime)),
+      poll_period_(std::max(poll_period, std::chrono::milliseconds{1})),
+      monitor_thread_(&Cia402StatusMonitor::monitorLoop_, this)
+{
+}
+
+Cia402StatusMonitor::~Cia402StatusMonitor()
+{
+    running_.store(false);
+    if (monitor_thread_.joinable()) {
+        monitor_thread_.join();
+    }
+}
+
+void Cia402StatusMonitor::addNode(const std::uint8_t node_id)
+{
+    {
+        std::lock_guard<std::mutex> lock(states_mutex_);
+        states_.try_emplace(node_id);
+    }
+    monitorNode_(node_id, false);
+}
+
+void Cia402StatusMonitor::setExpectedOperationEnabled(
+    const std::uint8_t node_id,
+    const bool expected)
+{
+    {
+        std::lock_guard<std::mutex> lock(states_mutex_);
+        states_[node_id].expected_operation_enabled = expected;
+    }
+    monitorNode_(node_id, expected);
+}
+
+bool Cia402StatusMonitor::isNodeOperational(const std::uint8_t node_id) const
+{
+    if (!bus_runtime_ || !bus_runtime_->isHealthy()) {
+        return false;
+    }
+    std::lock_guard<std::mutex> lock(states_mutex_);
+    const auto it = states_.find(node_id);
+    return it != states_.end() && it->second.has_last_sample &&
+           it->second.last_sample.read_ok &&
+           it->second.last_sample.transport_healthy &&
+           it->second.last_sample.operation_enabled &&
+           !it->second.last_sample.command_blocked;
+}
+
+void Cia402StatusMonitor::monitorLoop_()
+{
+    while (running_.load()) {
+        std::array<std::pair<std::uint8_t, bool>, 256> nodes{};
+        std::size_t node_count = 0;
+        {
+            std::lock_guard<std::mutex> lock(states_mutex_);
+            for (const auto& [node_id, state] : states_) {
+                if (node_count >= nodes.size()) {
+                    break;
+                }
+                nodes[node_count++] = {node_id, state.expected_operation_enabled};
+            }
+        }
+        for (std::size_t i = 0; i < node_count; ++i) {
+            monitorNode_(nodes[i].first, nodes[i].second);
+        }
+        std::this_thread::sleep_for(poll_period_);
+    }
+}
+
+void Cia402StatusMonitor::monitorNode_(
+    const std::uint8_t node_id,
+    const bool expected_operation_enabled)
+{
+    std::lock_guard<std::mutex> monitor_lock(monitor_mutex_);
+    StatusSample current;
+    readStatusSample_(node_id, expected_operation_enabled, current);
+
+    StatusSample previous;
+    bool had_previous = false;
+    {
+        std::lock_guard<std::mutex> lock(states_mutex_);
+        auto& state = states_[node_id];
+        had_previous = state.has_last_sample;
+        previous = state.last_sample;
+        state.last_sample = current;
+        state.has_last_sample = true;
+    }
+
+    if (!current.transport_healthy) {
+        return;
+    }
+    if (!current.read_ok) {
+        if (!had_previous || previous.read_ok) {
+            CMVR_LOG(ERROR) << "[Cia402StatusMonitor] failed to read node status snapshot"
+                            << ", node=" << static_cast<int>(node_id);
+        }
+        return;
+    }
+    if (had_previous && !previous.read_ok) {
+        CMVR_LOG(INFO) << "[Cia402StatusMonitor] node status snapshot recovered"
+                       << ", node=" << static_cast<int>(node_id);
+    }
+
+    reportErrorCodeTransition_(node_id, had_previous, previous, current);
+    reportStatuswordTransition_(node_id, had_previous, previous, current);
+}
+
+void Cia402StatusMonitor::reportErrorCodeTransition_(
+    const std::uint8_t node_id,
+    const bool had_previous,
+    const StatusSample& previous,
+    const StatusSample& current) const
+{
+    const bool error_code_changed =
+        !had_previous || !previous.read_ok ||
+        previous.error_code != current.error_code;
+    if (current.error_code != 0 && error_code_changed) {
+        CMVR_LOG(ERROR) << "[Cia402StatusMonitor] [error code] 0x"
+                        << std::hex << std::uppercase << std::setw(4)
+                        << std::setfill('0') << current.error_code
+                        << std::dec << std::nouppercase << std::setfill(' ')
+                        << ' ' << errorCodeDescription_(current.error_code)
+                        << ", node=" << static_cast<int>(node_id);
+    } else if (current.error_code == 0 && had_previous && previous.read_ok &&
+               previous.error_code != 0) {
+        CMVR_LOG(INFO) << "[Cia402StatusMonitor] [error code] recovered"
+                       << ", node=" << static_cast<int>(node_id)
+                       << ", previous_code=0x" << std::hex << std::uppercase
+                       << std::setw(4) << std::setfill('0') << previous.error_code
+                       << std::dec << std::nouppercase << std::setfill(' ');
+    }
+}
+
+void Cia402StatusMonitor::reportStatuswordTransition_(
+    const std::uint8_t node_id,
+    const bool had_previous,
+    const StatusSample& previous,
+    const StatusSample& current) const
+{
+    const bool status_changed =
+        !had_previous || !previous.read_ok ||
+        previous.status_problem != current.status_problem ||
+        (previous.statusword & 0x0888) != (current.statusword & 0x0888);
+    if (current.status_problem && status_changed) {
+        const auto status = cia402::statusword(current.statusword);
+        CMVR_LOG(WARNING) << "[Cia402StatusMonitor] [statusword] 0x"
+                          << std::hex << std::uppercase << std::setw(4)
+                          << std::setfill('0') << current.statusword
+                          << std::dec << std::nouppercase << std::setfill(' ')
+                          << ' ' << deviceStateName_(current.statusword)
+                          << ", node=" << static_cast<int>(node_id)
+                          << (status.fault != 0 ? ", fault" : "")
+                          << (status.warning != 0 ? ", warning" : "")
+                          << (status.internal_limit_active != 0
+                                  ? ", internal limit active"
+                                  : "")
+                          << (current.expected_operation_enabled &&
+                                      !current.operation_enabled
+                                  ? ", operation not enabled"
+                                  : "");
+    } else if (!current.status_problem && had_previous && previous.read_ok &&
+               previous.status_problem) {
+        CMVR_LOG(INFO) << "[Cia402StatusMonitor] [statusword] recovered"
+                       << ", node=" << static_cast<int>(node_id)
+                       << ", previous_statusword=0x" << std::hex << std::uppercase
+                       << std::setw(4) << std::setfill('0') << previous.statusword
+                       << std::dec << std::nouppercase << std::setfill(' ');
+    }
+}
+
+bool Cia402StatusMonitor::readStatusSample_(
+    const std::uint8_t node_id,
+    const bool expected_operation_enabled,
+    StatusSample& sample) const
+{
+    sample.expected_operation_enabled = expected_operation_enabled;
+    sample.transport_healthy = bus_runtime_ && bus_runtime_->isHealthy();
+    if (!sample.transport_healthy) {
+        return false;
+    }
+
+    std::array reads{
+        EthercatMotorBusRuntime::makePdoRead<std::uint16_t>(
+            node_id, msgs::CIA402_STATUS_WORD_6041, 0x00),
+        EthercatMotorBusRuntime::makePdoRead<std::uint16_t>(
+            node_id, msgs::CIA402_ERROR_CODE_603F, 0x00),
+        EthercatMotorBusRuntime::makePdoRead<std::int8_t>(
+            node_id, msgs::CIA402_MODE_DISPLAY_6061, 0x00),
+        EthercatMotorBusRuntime::makePdoRead<std::int32_t>(
+            node_id, msgs::CIA402_ACTUAL_POSITION_6064, 0x00),
+        EthercatMotorBusRuntime::makePdoRead<std::int32_t>(
+            node_id, msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00),
+        EthercatMotorBusRuntime::makePdoRead<std::int16_t>(
+            node_id, msgs::CIA402_ACTUAL_TORQUE_6077, 0x00),
+    };
+    if (!bus_runtime_->readPdosAtomic(reads.data(), reads.size())) {
+        return false;
+    }
+
+    sample.statusword = EthercatMotorBusRuntime::pdoReadValue<std::uint16_t>(reads[0]);
+    sample.error_code = EthercatMotorBusRuntime::pdoReadValue<std::uint16_t>(reads[1]);
+    sample.mode_display = EthercatMotorBusRuntime::pdoReadValue<std::int8_t>(reads[2]);
+    sample.actual_position = EthercatMotorBusRuntime::pdoReadValue<std::int32_t>(reads[3]);
+    sample.actual_velocity = EthercatMotorBusRuntime::pdoReadValue<std::int32_t>(reads[4]);
+    sample.actual_torque = EthercatMotorBusRuntime::pdoReadValue<std::int16_t>(reads[5]);
+    sample.read_ok = true;
+
+    const auto status = cia402::statusword(sample.statusword);
+    sample.operation_enabled = cia402::isOperationEnabled(status);
+    sample.status_problem = status.fault != 0 || status.warning != 0 ||
+                            status.internal_limit_active != 0 ||
+                            (expected_operation_enabled && !sample.operation_enabled);
+    sample.command_blocked = status.fault != 0 || status.warning != 0 ||
+                             sample.error_code != 0 ||
+                             (expected_operation_enabled && !sample.operation_enabled);
+    return true;
+}
+
+const char* Cia402StatusMonitor::deviceStateName_(const std::uint16_t statusword)
+{
+    if ((statusword & 0x004F) == 0x000F) {
+        return "FaultReactionActive";
+    }
+    if ((statusword & 0x004F) == 0x0008) {
+        return "Fault";
+    }
+    if ((statusword & 0x006F) == 0x0007) {
+        return "QuickStopActive";
+    }
+    const auto status = cia402::statusword(statusword);
+    if (cia402::isOperationEnabled(status)) {
+        return "OperationEnabled";
+    }
+    if (cia402::hasState(status, cia402::DeviceState::SwitchedOn)) {
+        return "SwitchedOn";
+    }
+    if (cia402::hasState(status, cia402::DeviceState::ReadyToSwitchOn)) {
+        return "ReadyToSwitchOn";
+    }
+    if (cia402::isSwitchOnDisabled(status)) {
+        return "SwitchOnDisabled";
+    }
+    return "NotReadyToSwitchOn";
+}
+
+const char* Cia402StatusMonitor::errorCodeDescription_(const std::uint16_t error_code)
+{
+    switch (error_code) {
+        case 0x0000:
+            return "no error";
+        case 0x2310:
+            return "continuous over-current";
+        case 0x3210:
+            return "DC bus over-voltage";
+        case 0x3220:
+            return "DC bus under-voltage";
+        case 0x4210:
+            return "device over-temperature";
+        case 0x4310:
+            return "drive over-temperature";
+        case 0x8611:
+            return "position following error";
+        default:
+            return "unknown or vendor-specific error";
+    }
+}
+
+} // namespace cmvr::device

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

@@ -1,8 +1,11 @@
 #include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h"
 
 #include <algorithm>
+#include <array>
 #include <cmath>
 #include <cstdint>
+#include <functional>
+#include <mutex>
 #include <limits>
 #include <map>
 #include <string>
@@ -107,8 +110,11 @@ bool EyouMotor::calibrateZeroQ()
         return false;
     }
 
+    const auto counts_per_joint_revolution = static_cast<std::int64_t>(
+        std::llround(encoder_counts_per_rev_ * gear_ratio_));
     std::int32_t zeroed_position = 0;
-    if (!vendor_adapter_->calibrateZero(node_id_, zeroed_position)) {
+    if (!vendor_adapter_->calibrateZero(node_id_, counts_per_joint_revolution,
+                                        zeroed_position)) {
         CMVR_LOG(ERROR) << "[EyouMotor] zero calibration failed: " << info_.joint_name;
         return false;
     }
@@ -121,6 +127,102 @@ bool EyouMotor::calibrateZeroQ()
     return true;
 }
 
+bool EyouMotor::commandCyclicPositionsAtomic(
+    const std::vector<std::shared_ptr<AbstractMotor>>& motors,
+    const std::vector<double>& positions,
+    const std::vector<double>& velocities)
+{
+    if (motors.empty() || motors.size() != positions.size() ||
+        motors.size() != velocities.size() || motors.size() > 256) {
+        return false;
+    }
+
+    std::array<EyouMotor*, 256> lock_order{};
+    std::array<Cia402Protocol::CyclicPositionCommand, 256> commands{};
+    std::shared_ptr<Cia402Protocol> protocol;
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        const auto motor = std::dynamic_pointer_cast<EyouMotor>(motors[i]);
+        if (!motor || !motor->hasDependencies_() || !motor->hasValidConversion_()) {
+            return false;
+        }
+        if (!protocol) {
+            protocol = motor->cia402_protocol_;
+        } else if (protocol.get() != motor->cia402_protocol_.get()) {
+            CMVR_LOG(ERROR) << "[EyouMotor] batch target motors belong to different "
+                            << "CiA402 protocols";
+            return false;
+        }
+        for (std::size_t previous = 0; previous < i; ++previous) {
+            if (lock_order[previous] == motor.get()) {
+                return false;
+            }
+        }
+        lock_order[i] = motor.get();
+        commands[i] = Cia402Protocol::CyclicPositionCommand{
+            motor->node_id_, positions[i], velocities[i]};
+    }
+
+    std::sort(lock_order.begin(), lock_order.begin() + motors.size(),
+              std::less<EyouMotor*>{});
+    std::array<std::unique_lock<std::mutex>, 256> locks{};
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        locks[i] = std::unique_lock<std::mutex>(lock_order[i]->mtx_);
+    }
+    return protocol && protocol->commandCyclicPositionsAtomic(commands.data(), motors.size());
+}
+
+bool EyouMotor::readFeedbacksAtomic(
+    const std::vector<std::shared_ptr<AbstractMotor>>& motors,
+    std::vector<double>& positions,
+    std::vector<double>& velocities)
+{
+    if (motors.empty() || motors.size() > 256) {
+        return false;
+    }
+
+    std::array<EyouMotor*, 256> lock_order{};
+    std::array<Cia402Protocol::MotorFeedback, 256> feedbacks{};
+    std::shared_ptr<Cia402Protocol> protocol;
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        const auto motor = std::dynamic_pointer_cast<EyouMotor>(motors[i]);
+        if (!motor || !motor->hasDependencies_() || !motor->hasValidConversion_()) {
+            return false;
+        }
+        if (!protocol) {
+            protocol = motor->cia402_protocol_;
+        } else if (protocol.get() != motor->cia402_protocol_.get()) {
+            CMVR_LOG(ERROR) << "[EyouMotor] batch feedback motors belong to different "
+                            << "CiA402 protocols";
+            return false;
+        }
+        for (std::size_t previous = 0; previous < i; ++previous) {
+            if (lock_order[previous] == motor.get()) {
+                return false;
+            }
+        }
+        lock_order[i] = motor.get();
+        feedbacks[i].node_id = motor->node_id_;
+    }
+
+    std::sort(lock_order.begin(), lock_order.begin() + motors.size(),
+              std::less<EyouMotor*>{});
+    std::array<std::unique_lock<std::mutex>, 256> locks{};
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        locks[i] = std::unique_lock<std::mutex>(lock_order[i]->mtx_);
+    }
+    if (!protocol || !protocol->readFeedbacksAtomic(feedbacks.data(), motors.size())) {
+        return false;
+    }
+
+    positions.resize(motors.size());
+    velocities.resize(motors.size());
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        positions[i] = feedbacks[i].q;
+        velocities[i] = feedbacks[i].qd;
+    }
+    return true;
+}
+
 bool EyouMotor::brakeRelease()
 {
     std::scoped_lock lock(mtx_);

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

@@ -9,6 +9,7 @@
 
 #include "cmvr/msgs/cia402.pb.h"
 #include "common/base/logging/logger.h"
+#include "common/math/support_functions.h"
 #include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h"
 
 namespace cmvr::device {
@@ -93,96 +94,250 @@ bool EyouMotorAdapter::writeVelocityLimit(const std::uint8_t node_id,
 }
 
 bool EyouMotorAdapter::calibrateZero(const std::uint8_t node_id,
+                                      const std::int64_t counts_per_joint_revolution,
                                       std::int32_t& zeroed_position)
 {
     if (!bus_runtime_) {
         return false;
     }
 
+    const auto& zero_config = bus_runtime_->config().zero_calibration();
+    const auto home_offset_timeout =
+        std::chrono::milliseconds{zero_config.timeout_ms()};
+    const auto home_offset_poll_period =
+        std::chrono::milliseconds{zero_config.poll_period_ms()};
+    const auto home_offset_stable_samples = zero_config.stable_sample_count();
+    const auto home_offset_position_tolerance_counts =
+        zero_config.position_tolerance_counts();
+    const auto home_offset_stable_delta_counts =
+        zero_config.stable_delta_counts();
+
+    std::uint32_t original_soft_limit_state = 0;
+    std::int32_t original_home_offset = 0;
+    std::int32_t original_position = 0;
+    if (!bus_runtime_->readSdo<std::uint32_t>(
+            node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
+            0x00, original_soft_limit_state) ||
+        !bus_runtime_->readSdo<std::int32_t>(
+            node_id, msgs::CIA402_HOME_OFFSET_607C,
+            0x00, original_home_offset) ||
+        !bus_runtime_->readSdo<std::int32_t>(
+            node_id, msgs::CIA402_ACTUAL_POSITION_6064,
+            0x00, original_position)) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to snapshot calibration state, node="
+                        << static_cast<int>(node_id);
+        return false;
+    }
+
+    // EYOU applies HomeOffset additively, so clearing it exposes this raw position.
+    const auto expected_cleared_position_wide =
+        static_cast<std::int64_t>(original_position) -
+        static_cast<std::int64_t>(original_home_offset);
+    if (expected_cleared_position_wide < std::numeric_limits<std::int32_t>::min() ||
+        expected_cleared_position_wide > std::numeric_limits<std::int32_t>::max()) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] cleared position would overflow, node="
+                        << static_cast<int>(node_id)
+                        << ", original_position=" << original_position
+                        << ", original_home_offset=" << original_home_offset;
+        return false;
+    }
+    const auto expected_cleared_position =
+        static_cast<std::int32_t>(expected_cleared_position_wide);
+
+    const auto write_home_offset = [&](const std::int32_t value) {
+        return bus_runtime_->writeSdo<std::int32_t>(
+            node_id, msgs::CIA402_HOME_OFFSET_607C, 0x00, value);
+    };
+    const auto save_parameters = [&]() {
+        return bus_runtime_->writeSdo<std::uint32_t>(
+            node_id, eyou::EYOU_STORE_PARAMETERS_1010,
+            0x01, 0x65766173);
+    };
+    const auto wait_for_soft_limit = [&](const std::uint32_t expected_state) {
+        const auto deadline = std::chrono::steady_clock::now() + home_offset_timeout;
+        do {
+            std::uint32_t actual_soft_limit_state = 0;
+            if (bus_runtime_->readSdo<std::uint32_t>(
+                    node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
+                    0x00, actual_soft_limit_state) &&
+                actual_soft_limit_state == expected_state) {
+                return true;
+            }
+            std::this_thread::sleep_for(home_offset_poll_period);
+        } while (std::chrono::steady_clock::now() < deadline);
+        return false;
+    };
+    const auto restore_soft_limit = [&]() {
+        return bus_runtime_->writeSdo<std::uint32_t>(
+                   node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
+                   0x00, original_soft_limit_state) &&
+               wait_for_soft_limit(original_soft_limit_state);
+    };
+    const auto wait_for_position = [&](const char* phase,
+                                       const std::int32_t expected_offset,
+                                       const std::int32_t expected_position,
+                                       std::int32_t& observed_position) {
+        const auto deadline = std::chrono::steady_clock::now() + home_offset_timeout;
+        std::uint32_t stable_samples = 0;
+        bool has_previous_position = false;
+        std::int32_t previous_position = 0;
+        std::int32_t observed_offset = 0;
+
+        do {
+            const bool read_ok =
+                bus_runtime_->readSdo<std::int32_t>(
+                    node_id, msgs::CIA402_HOME_OFFSET_607C,
+                    0x00, observed_offset) &&
+                bus_runtime_->readSdo<std::int32_t>(
+                    node_id, msgs::CIA402_ACTUAL_POSITION_6064,
+                    0x00, observed_position);
+            const bool position_stable =
+                !has_previous_position ||
+                SupportFunctions::cyclicAbsoluteDifference(
+                    observed_position, previous_position,
+                    counts_per_joint_revolution) <= home_offset_stable_delta_counts;
+            const bool sample_matches =
+                read_ok && observed_offset == expected_offset &&
+                SupportFunctions::cyclicAbsoluteDifference(
+                    observed_position, expected_position,
+                    counts_per_joint_revolution) <= home_offset_position_tolerance_counts &&
+                position_stable;
+
+            stable_samples = sample_matches ? stable_samples + 1 : 0;
+            if (stable_samples >= home_offset_stable_samples) {
+                return true;
+            }
+
+            if (read_ok) {
+                previous_position = observed_position;
+                has_previous_position = true;
+            }
+            std::this_thread::sleep_for(home_offset_poll_period);
+        } while (std::chrono::steady_clock::now() < deadline);
+
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] timed out waiting for home offset state, node="
+                        << static_cast<int>(node_id)
+                        << ", phase=" << phase
+                        << ", expected_offset=" << expected_offset
+                        << ", actual_offset=" << observed_offset
+                        << ", expected_position=" << expected_position
+                        << ", actual_position=" << observed_position
+                        << ", cyclic_position_distance="
+                        << SupportFunctions::cyclicAbsoluteDifference(
+                               observed_position, expected_position,
+                               counts_per_joint_revolution)
+                        << ", counts_per_joint_revolution="
+                        << counts_per_joint_revolution
+                        << ", stable_samples=" << stable_samples;
+        return false;
+    };
+    // Follow EYOU's required clear -> set -> save sequence during rollback too.
+    const auto rollback = [&](const char* failed_phase) {
+        const bool clear_written = write_home_offset(0);
+        std::int32_t cleared_position = 0;
+        const bool clear_applied =
+            clear_written &&
+            wait_for_position("rollback_clear_home_offset", 0,
+                              expected_cleared_position, cleared_position);
+        const bool offset_written = write_home_offset(original_home_offset);
+        std::int32_t restored_position = 0;
+        const bool offset_applied =
+            offset_written &&
+            wait_for_position("rollback_apply_home_offset", original_home_offset,
+                              original_position, restored_position);
+        const bool parameters_saved = offset_written && save_parameters();
+        const bool saved_state_confirmed =
+            parameters_saved &&
+            wait_for_position("rollback_save_home_offset", original_home_offset,
+                              original_position, restored_position);
+        const bool soft_limit_restored = restore_soft_limit();
+        const bool rollback_ok = clear_applied && offset_applied &&
+                                 saved_state_confirmed && soft_limit_restored;
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] calibration failed and original state was "
+            << (rollback_ok ? "restored" : "not fully restored")
+            << ", node=" << static_cast<int>(node_id)
+            << ", phase=" << failed_phase
+            << ", original_home_offset=" << original_home_offset
+            << ", original_soft_limit_state=" << original_soft_limit_state
+            << ", clear_applied=" << clear_applied
+            << ", offset_applied=" << offset_applied
+            << ", saved_state_confirmed=" << saved_state_confirmed
+            << ", soft_limit_restored=" << soft_limit_restored;
+        return false;
+    };
+
     if (!bus_runtime_->writeSdo<std::uint32_t>(node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
                                                0x00, 0)) {
+        const bool soft_limit_restored = restore_soft_limit();
         CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to disable software position "
                         << "limit before home offset calibration, node="
-                        << static_cast<int>(node_id);
+                        << static_cast<int>(node_id)
+                        << ", soft_limit_restored=" << soft_limit_restored;
         return false;
     }
+    if (!wait_for_soft_limit(0)) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] software position limit did not disable, node="
+                        << static_cast<int>(node_id);
+        return rollback("disable_soft_limit");
+    }
 
-    if (!bus_runtime_->writeSdo<std::int32_t>(node_id, msgs::CIA402_HOME_OFFSET_607C,
-                                              0x00, 0)) {
+    if (!write_home_offset(0)) {
         CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to clear home offset, node="
                         << static_cast<int>(node_id);
-        return false;
+        return rollback("clear_home_offset");
     }
-    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 (!wait_for_position("clear_home_offset", 0,
+                           expected_cleared_position, actual_position)) {
+        return rollback("wait_for_cleared_position");
     }
-
     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;
+        return rollback("negate_actual_position");
     }
 
     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)) {
+    if (!write_home_offset(home_offset)) {
         CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write home offset, node="
                         << static_cast<int>(node_id)
                         << ", home_offset=" << home_offset;
-        return false;
+        return rollback("write_home_offset");
     }
 
-    if (!bus_runtime_->writeSdo<std::uint32_t>(
-            node_id, eyou::EYOU_STORE_PARAMETERS_1010,
-            0x01,
-            0x65766173)) {
+    if (!wait_for_position("apply_home_offset", home_offset, 0,
+                           zeroed_position)) {
+        return rollback("wait_for_zero_before_save");
+    }
+
+    if (!save_parameters()) {
         CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to save home offset parameter, node="
                         << static_cast<int>(node_id);
-        return false;
+        return rollback("save_parameters");
     }
-    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 (!wait_for_position("save_home_offset", home_offset, 0,
+                           zeroed_position)) {
+        return rollback("wait_for_zero_after_save");
     }
-    if (home_offset_readback != home_offset) {
-        CMVR_LOG(ERROR) << "[EyouMotorAdapter] home offset readback mismatch, node="
+
+    if (!restore_soft_limit()) {
+        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to restore software position "
+                        << "limit after home offset calibration, node="
                         << static_cast<int>(node_id)
-                        << ", expected=" << home_offset
-                        << ", actual=" << home_offset_readback;
-        return false;
+                        << ", original_soft_limit_state=" << original_soft_limit_state;
+        return rollback("restore_soft_limit");
     }
 
-    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
+    CMVR_LOG(INFO) << "[EyouMotorAdapter] home offset calibration completed, node="
+                   << static_cast<int>(node_id)
+                   << ", original_home_offset=" << original_home_offset
+                   << ", cleared_position=" << actual_position
                    << ", home_offset=" << home_offset
-                        << ", home_offset_readback=" << home_offset_readback
-                        << ", actual_position_after=" << zeroed_position
-                        << ", tolerance_counts=" << 10000;
-        return false;
-    }
-
+                   << ", zeroed_position=" << zeroed_position
+                   << ", soft_limit_state=" << original_soft_limit_state;
     return true;
 }
 

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

@@ -7,6 +7,8 @@
 #include <limits>
 #include <memory>
 #include <thread>
+#include <utility>
+#include <vector>
 
 #include <gtest/gtest.h>
 
@@ -21,17 +23,21 @@ namespace {
 
 constexpr const char* kMotorManagerId = "ethercat_motors";
 constexpr const char* kMotorConfigFile =
-    "devices/motor/ethercat_motors_two_real_test.pb.txt";
-constexpr std::array<int, 5> kFourMotorIds{1, 2, 3, 4,5};
+    "devices/motor/ethercat_motors_four_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{200000000};
+constexpr std::chrono::milliseconds kFourMotorTrajectoryDuration{20000};
 constexpr double kPi = 3.14159265358979323846;
-constexpr double kFourMotorAmplitudeRad = 3.0;
-constexpr double kFourMotorPeriodS =10;
-constexpr std::array<double, 5> kFourMotorPhaseRad{0.0, 0.0, 0.0, 0.0,0};
+constexpr double kRaisedCosineCoefficientRad = 0.2;
+constexpr double kFourMotorPeriodS = 2.0;
+constexpr std::array<double, 4> kFourMotorPhaseRad{0.0, 0.0, 0.0, 0.0};
+constexpr double kMinimumPositionExcursionRad = 0.2;
+constexpr double kMaximumAbsoluteTrackingErrorRad = 0.15;
+constexpr double kMaximumRmsTrackingErrorRad = 0.08;
+constexpr double kMaximumErrorSpreadRad = 0.10;
+constexpr double kFinalPositionToleranceRad = 0.05;
 
 class DeviceManagerDestroyGuard {
 public:
@@ -41,6 +47,61 @@ public:
     }
 };
 
+class MotorManagerStopGuard {
+public:
+    explicit MotorManagerStopGuard(std::shared_ptr<MotorManager> motor_manager)
+        : motor_manager_(std::move(motor_manager))
+    {
+    }
+
+    ~MotorManagerStopGuard()
+    {
+        if (motor_manager_ && !motor_manager_->stop()) {
+            std::cerr << "failed to stop motor manager during test cleanup" << std::endl;
+        }
+    }
+
+private:
+    std::shared_ptr<MotorManager> motor_manager_;
+};
+
+class MultiMotorSafetyGuard {
+public:
+    explicit MultiMotorSafetyGuard(
+        const std::vector<std::shared_ptr<AbstractMotor>>& motors)
+        : motors_(motors)
+    {
+    }
+
+    ~MultiMotorSafetyGuard()
+    {
+        if (armed_) {
+            stop();
+        }
+    }
+
+    bool stop()
+    {
+        bool all_ok = true;
+        for (auto it = motors_.rbegin(); it != motors_.rend(); ++it) {
+            if (*it && !(*it)->quickStop()) {
+                all_ok = false;
+            }
+        }
+        for (auto it = motors_.rbegin(); it != motors_.rend(); ++it) {
+            if (*it && !(*it)->torqueOff()) {
+                all_ok = false;
+            }
+        }
+        armed_ = !all_ok;
+        return all_ok;
+    }
+
+private:
+    const std::vector<std::shared_ptr<AbstractMotor>>& motors_;
+    bool armed_{true};
+};
+
 struct TrackingErrorStats {
     std::int64_t sample_count{0};
     double sum_error{0.0};
@@ -140,31 +201,16 @@ TEST(EyouMotorDeviceManagerRealTest, InitFourEthercatMotorsAndPrintState)
         DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig());
     auto motor_manager = device_manager.getDevice<MotorManager>(kMotorManagerId);
     ASSERT_NE(motor_manager, nullptr);
+    MotorManagerStopGuard motor_manager_stop_guard(motor_manager);
 
-    // 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));
+    for (const int motor_id : kFourMotorIds) {
+        auto motor = motor_manager->getMotor(static_cast<std::uint8_t>(motor_id));
+        ASSERT_NE(motor, nullptr);
+        printMotorState(motor_id, motor);
     }
-    motor->quickStop();
-
 }
 
-TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
+TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionRaisedCosineTrajectory)
 {
     ConfigHelper::setConfigRootFromFile("cmvr-es/config/cmvr_es.pb.txt");
     DeviceManagerDestroyGuard guard;
@@ -173,16 +219,21 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
         DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig());
     auto motor_manager = device_manager.getDevice<MotorManager>(kMotorManagerId);
     ASSERT_NE(motor_manager, nullptr);
+    MotorManagerStopGuard motor_manager_stop_guard(motor_manager);
 
-    std::array<std::shared_ptr<AbstractMotor>, kFourMotorIds.size()> motors;
+    std::vector<std::shared_ptr<AbstractMotor>> motors(kFourMotorIds.size());
     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]);
     }
+    MultiMotorSafetyGuard safety_guard(motors);
 
     std::cout << "calibrate zero for four EtherCAT motors" << std::endl;
+    for (const auto& motor : motors) {
+        ASSERT_TRUE(motor->torqueOff());
+    }
     for (std::size_t i = 0; i < motors.size(); ++i) {
         const int motor_id = kFourMotorIds[i];
         std::cout << "before calibrateZeroQ: ";
@@ -196,48 +247,64 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
         ASSERT_TRUE(motor->torqueOn());
     }
     for (const auto& motor : motors) {
-        motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
+        ASSERT_TRUE(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();
-    }
+    std::vector<double> center_q;
+    std::vector<double> actual_qd;
+    ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, center_q, actual_qd));
 
     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
+              << " ms, raised_cosine_coefficient=" << kRaisedCosineCoefficientRad
+              << " rad, position_excursion=" << 2.0 * kRaisedCosineCoefficientRad
               << " rad, period=" << kFourMotorPeriodS
               << " s" << std::endl;
 
     const auto start_time = std::chrono::steady_clock::now();
-    const auto total_ticks = kFourMotorTrajectoryDuration / kCyclicCommandPeriod;
+    const auto end_time = start_time + kFourMotorTrajectoryDuration;
+    auto next_command_time = start_time;
+    auto next_stats_time = start_time;
+    std::uint64_t missed_command_deadlines = 0;
     std::array<TrackingErrorStats, kFourMotorIds.size()> error_stats;
+    std::vector<double> target_q(motors.size(), 0.0);
+    std::vector<double> target_qd(motors.size(), 0.0);
+    std::vector<double> actual_q;
+    std::array<double, kFourMotorIds.size()> minimum_actual_q{};
+    std::array<double, kFourMotorIds.size()> maximum_actual_q{};
+    std::copy(center_q.begin(), center_q.end(), minimum_actual_q.begin());
+    std::copy(center_q.begin(), center_q.end(), maximum_actual_q.begin());
     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;
+    while (true) {
+        std::this_thread::sleep_until(next_command_time);
+        const auto now = std::chrono::steady_clock::now();
+        if (now > end_time) {
+            break;
+        }
+        const double t_s = std::chrono::duration<double>(now - start_time).count();
 
-        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 * std::sin(theta);
-            target_qd[i] = kFourMotorAmplitudeRad * omega * std::cos(theta);
-            ASSERT_TRUE(motors[i]->commandCyclicPosition(target_q[i], target_qd[i]));
+            target_q[i] =
+                center_q[i] + kRaisedCosineCoefficientRad * (1.0 - std::cos(theta));
+            target_qd[i] = kRaisedCosineCoefficientRad * omega * std::sin(theta);
         }
+        ASSERT_TRUE(motor_manager->commandCyclicPositionsAtomic(motors, target_q, target_qd));
 
-        if (elapsed.count() % kStatsSamplePeriod.count() == 0) {
+        if (now >= next_stats_time) {
+            ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, actual_q, actual_qd));
             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];
+                const double error = actual_q[i] - target_q[i];
                 error_stats[i].add(error, theta);
+                minimum_actual_q[i] = std::min(minimum_actual_q[i], actual_q[i]);
+                maximum_actual_q[i] = std::max(maximum_actual_q[i], actual_q[i]);
                 min_error = std::min(min_error, error);
                 max_error = std::max(max_error, error);
             }
@@ -245,31 +312,24 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
             max_error_spread_rad = std::max(max_error_spread_rad, error_spread);
             sum_error_spread_sq += error_spread * error_spread;
             ++error_spread_sample_count;
+            next_stats_time = now + kStatsSamplePeriod;
         }
 
-        // 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);
+        next_command_time += kCyclicCommandPeriod;
+        const auto command_complete_time = std::chrono::steady_clock::now();
+        if (next_command_time <= command_complete_time) {
+            const auto skipped_periods =
+                (command_complete_time - next_command_time) / kCyclicCommandPeriod + 1;
+            missed_command_deadlines += static_cast<std::uint64_t>(skipped_periods);
+            next_command_time += skipped_periods * 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::copy(center_q.begin(), center_q.end(), target_q.begin());
+    std::fill(target_qd.begin(), target_qd.end(), 0.0);
+    ASSERT_TRUE(motor_manager->commandCyclicPositionsAtomic(motors, target_q, target_qd));
+    std::this_thread::sleep_for(kHoldAfterTrajectoryDuration);
+    ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, actual_q, actual_qd));
 
     std::cout << "after four motor CSP trajectory" << std::endl;
     for (std::size_t i = 0; i < motors.size(); ++i) {
@@ -284,6 +344,7 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
     std::cout << "four motor CSP tracking error statistics, sample_period="
               << kStatsSamplePeriod.count()
               << " ms, samples=" << error_stats.front().sample_count
+              << ", missed_command_deadlines=" << missed_command_deadlines
               << ", max_error_spread=" << max_error_spread_rad
               << " rad, rms_error_spread=" << rms_error_spread
               << " rad" << std::endl;
@@ -303,7 +364,19 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
                   << " rad (" << radToDeg(relative_phase)
                   << " deg, " << relative_phase_ms
                   << " ms)" << std::endl;
+        EXPECT_GE(maximum_actual_q[i] - minimum_actual_q[i],
+                  kMinimumPositionExcursionRad)
+            << "motor_id=" << kFourMotorIds[i] << " did not complete enough motion";
+        EXPECT_LE(error_stats[i].max_abs_error,
+                  kMaximumAbsoluteTrackingErrorRad)
+            << "motor_id=" << kFourMotorIds[i] << " exceeded maximum tracking error";
+        EXPECT_LE(error_stats[i].rms(), kMaximumRmsTrackingErrorRad)
+            << "motor_id=" << kFourMotorIds[i] << " exceeded RMS tracking error";
+        EXPECT_NEAR(actual_q[i], center_q[i], kFinalPositionToleranceRad)
+            << "motor_id=" << kFourMotorIds[i] << " did not return to its start position";
     }
+    EXPECT_LE(max_error_spread_rad, kMaximumErrorSpreadRad);
+    EXPECT_TRUE(safety_guard.stop());
 }
 
 } // namespace cmvr::device

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

@@ -23,7 +23,6 @@ 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};
@@ -46,12 +45,18 @@ config::MotorGroupConfig createSingleSlaveGroup()
     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(2000);
+    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* zero_calibration = ethercat->mutable_zero_calibration();
+    zero_calibration->set_timeout_ms(2000);
+    zero_calibration->set_poll_period_ms(10);
+    zero_calibration->set_stable_sample_count(5);
+    zero_calibration->set_position_tolerance_counts(10000);
+    zero_calibration->set_stable_delta_counts(1000);
+
     auto* dc = ethercat->mutable_dc();
     dc->set_enable(false);
     dc->set_reference_motor_id(kMotorId);
@@ -113,10 +118,10 @@ 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_q_lb(-36.14);
+    config.set_limit_q_ub(36.14);
     config.set_limit_qd(10.0);
-    config.set_limit_qdd(10.0);
+    config.set_limit_qdd(100.0);
     config.set_encoder_counts_per_rev(kEncoderCountsPerMotorRev);
     config.set_gear_ratio(kDefaultGearRatio);
     return config;
@@ -160,7 +165,6 @@ void printRawEthercatFeedback(const char* label,
     std::int32_t actual_position = 0;
     std::int32_t actual_velocity = 0;
     std::int16_t actual_torque = 0;
-    std::int16_t actual_current = 0;
     std::uint16_t error_code = 0;
 
     runtime->readPdo<std::uint16_t>(kMotorId, msgs::CIA402_STATUS_WORD_6041, 0x00, statusword);
@@ -171,8 +175,6 @@ void printRawEthercatFeedback(const char* label,
                                    actual_velocity);
     runtime->readPdo<std::int16_t>(kMotorId, msgs::CIA402_ACTUAL_TORQUE_6077, 0x00,
                                    actual_torque);
-    runtime->readPdo<std::int16_t>(kMotorId, msgs::CIA402_ACTUAL_CURRENT_6078, 0x00,
-                                   actual_current);
     runtime->readPdo<std::uint16_t>(kMotorId, msgs::CIA402_ERROR_CODE_603F, 0x00, error_code);
 
     std::cout << label
@@ -181,7 +183,6 @@ void printRawEthercatFeedback(const char* label,
               << ", actual_position=" << actual_position
               << ", actual_velocity=" << actual_velocity
               << ", actual_torque=" << actual_torque
-              << ", actual_current=" << actual_current
               << ", error_code=" << hex16(error_code)
               << std::endl;
 }
@@ -238,6 +239,9 @@ TEST(EyouMotorRealTest, CalibrateZeroQPrintBeforeAndAfter)
     ASSERT_TRUE(motor->calibrateZeroQ());
     printMotorState("after calibrateZeroQ", *motor);
     ASSERT_TRUE(motor->torqueOn());
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION));
+    ASSERT_TRUE(motor->commandProfilePosition(1.5,0.8,3.0));
+    sampleMotorState(*motor, kFeedbackSampleDuration);
 }
 
 TEST(EyouMotorRealTest, CommandProfilePosition)
@@ -250,8 +254,7 @@ TEST(EyouMotorRealTest, CommandProfilePosition)
     ASSERT_NE(motor, nullptr);
 
     ASSERT_TRUE(motor->torqueOn());
-    motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
-    std::this_thread::sleep_for(kModeSettleDelay);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION));
 
     ASSERT_TRUE(motor->commandProfilePosition(-3.0, 0.5, 1.0));
     sampleMotorState(*motor, kFeedbackSampleDuration);
@@ -266,9 +269,10 @@ TEST(EyouMotorRealTest, CommandProfileVelocity)
     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);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY));
 
     std::cout << "motor.commandProfileVelocity(0.3 rad/s, 1.0 rad/s^2)" << std::endl;
     ASSERT_TRUE(motor->commandProfileVelocity(0.3, 1.0));
@@ -288,21 +292,20 @@ TEST(EyouMotorRealTest, CommandCyclicPosition)
     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);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION));
 
-    const std::chrono::milliseconds trajectory_duration{15000};
+    const std::chrono::milliseconds trajectory_duration{12000};
     const double period_s = 6.0;
-    const double amplitude_rad = 3;
-    const double phase_rad = 0.0;
+    const double excursion_rad = 3.0;
     const double center_q = motor->getQ();
     const double omega = 2.0 * kPi / period_s;
 
-    std::cout << "motor.commandCyclicPosition(sin), center_q=" << center_q
+    std::cout << "motor.commandCyclicPosition(raised cosine), center_q=" << center_q
               << " rad, period=" << period_s
-              << " s, amplitude=" << amplitude_rad
-              << " rad, phase=" << phase_rad
+              << " s, excursion=" << excursion_rad
               << " rad, command_period=" << kCyclicCommandPeriod.count()
               << " ms" << std::endl;
 
@@ -311,9 +314,11 @@ TEST(EyouMotorRealTest, CommandCyclicPosition)
     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);
+        const double theta = omega * t_s;
+        const double target_q =
+            center_q + 0.5 * excursion_rad * (1.0 - std::cos(theta));
+        const double target_qd =
+            0.5 * excursion_rad * omega * std::sin(theta);
 
         ASSERT_TRUE(motor->commandCyclicPosition(target_q, target_qd));
         if (elapsed.count() % kCommandSamplePeriod.count() == 0) {
@@ -336,19 +341,22 @@ TEST(EyouMotorRealTest, CommandCyclicVelocity)
     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);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY));
 
-    const std::chrono::milliseconds trajectory_duration{15000};
-    const double period_s = 6.0;
-    const double velocity_amplitude_rad_s = 5.0;
+    const std::chrono::milliseconds trajectory_duration{12000};
+    const double period_s = 5.0;
+    const double excursion_rad = 5.0;
     const double phase_rad = 0.0;
     const double omega = 2.0 * kPi / period_s;
+    const double velocity_amplitude_rad_s = 0.5 * excursion_rad * omega;
 
     std::cout << "motor.commandCyclicVelocity(sin), period=" << period_s
               << " s, velocity_amplitude=" << velocity_amplitude_rad_s
-              << " rad/s, phase=" << phase_rad
+              << " rad/s, excursion=" << excursion_rad
+              << " rad, phase=" << phase_rad
               << " rad, command_period=" << kCyclicCommandPeriod.count()
               << " ms" << std::endl;
 
@@ -366,6 +374,7 @@ TEST(EyouMotorRealTest, CommandCyclicVelocity)
                       << " ms, target_qd=" << target_qd
                       << " rad/s";
             printMotorState("", *motor);
+            printRawEthercatFeedback("raw feedback", runtime);
         }
         std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
     }
@@ -373,6 +382,7 @@ TEST(EyouMotorRealTest, CommandCyclicVelocity)
     std::cout << "motor.commandCyclicVelocity(0 rad/s)" << std::endl;
     ASSERT_TRUE(motor->commandCyclicVelocity(0.0));
     sampleMotorState(*motor, std::chrono::milliseconds{500});
+    printRawEthercatFeedback("raw feedback after stop", runtime);
 }
 
 TEST(EyouMotorRealTest, QuickStopAfterTwoSeconds)
@@ -387,8 +397,7 @@ TEST(EyouMotorRealTest, QuickStopAfterTwoSeconds)
     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);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY));
 
     const std::chrono::milliseconds run_duration{2000};
     const double period_s = 6.0;
@@ -443,13 +452,12 @@ TEST(EyouMotorRealTest, QuickStopInProfilePosition)
     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);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION));
 
     const std::chrono::milliseconds quick_stop_time{1000};
     const double start_q = motor->getQ();
-    const double target_q = nearbySafeTarget(start_q, 5.0);
-    const double max_qd = 5.0;
+    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
@@ -483,8 +491,7 @@ TEST(EyouMotorRealTest, QuickStopInProfileVelocity)
     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);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY));
 
     const std::chrono::milliseconds quick_stop_time{2000};
     const double target_qd = motor->getQ() > 0.0 ? -2.0 : 2.0;
@@ -520,12 +527,11 @@ TEST(EyouMotorRealTest, QuickStopInCyclicPosition)
     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);
+    ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION));
 
     const std::chrono::milliseconds run_duration{2000};
     const double start_q = motor->getQ();
-    const double target_qd = start_q > 0.0 ? -5.0 : 5.0;
+    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
@@ -554,7 +560,7 @@ TEST(EyouMotorRealTest, QuickStopInCyclicPosition)
     }
 
     std::cout << "motor.quickStop()" << std::endl;
-    // ASSERT_TRUE(motor->quickStop());
+    ASSERT_TRUE(motor->quickStop());
     printMotorState("after quickStop", *motor);
     printRawEthercatFeedback("raw feedback", runtime);
     sampleMotorState(*motor, std::chrono::milliseconds{1000});

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

@@ -21,7 +21,7 @@ public:
     std::string typeName() const override { return "MujocoMotor"; }
     bool init() override;
 
-    void setMode(msgs::RunMode mode) override;
+    bool setMode(msgs::RunMode mode) override;
     msgs::RunMode getMode() override;
     bool torqueOn() override;
     bool torqueOff() override;
@@ -45,7 +45,8 @@ public:
     double getQ() override;
     double getQd() override;
 
-    static bool setTargetsAtomic(const std::vector<std::shared_ptr<MujocoMotor>>& motors,
+    static bool commandCyclicPositionsAtomic(
+        const std::vector<std::shared_ptr<AbstractMotor>>& motors,
         const std::vector<double>& positions,
         const std::vector<double>& velocities);
 

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

@@ -42,10 +42,11 @@ bool MujocoMotor::init()
     return true;
 }
 
-void MujocoMotor::setMode(const msgs::RunMode mode)
+bool MujocoMotor::setMode(const msgs::RunMode mode)
 {
     std::scoped_lock lock(mtx_);
     mode_ = mode;
+    return true;
 }
 
 msgs::RunMode MujocoMotor::getMode()
@@ -216,7 +217,8 @@ double MujocoMotor::getQd()
     return qd;
 }
 
-bool MujocoMotor::setTargetsAtomic(const std::vector<std::shared_ptr<MujocoMotor>>& motors,
+bool MujocoMotor::commandCyclicPositionsAtomic(
+    const std::vector<std::shared_ptr<AbstractMotor>>& motors,
     const std::vector<double>& positions,
     const std::vector<double>& velocities)
 {
@@ -233,7 +235,7 @@ bool MujocoMotor::setTargetsAtomic(const std::vector<std::shared_ptr<MujocoMotor
     clamped_velocities.reserve(motors.size());
 
     for (std::size_t i = 0; i < motors.size(); ++i) {
-        const auto& motor = motors[i];
+        const auto motor = std::dynamic_pointer_cast<MujocoMotor>(motors[i]);
         if (!motor) {
             return false;
         }
@@ -263,9 +265,13 @@ bool MujocoMotor::setTargetsAtomic(const std::vector<std::shared_ptr<MujocoMotor
     }
 
     for (std::size_t i = 0; i < motors.size(); ++i) {
-        std::scoped_lock lock(motors[i]->mtx_);
-        motors[i]->target_q_ = clamped_positions[i];
-        motors[i]->mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
+        const auto motor = std::dynamic_pointer_cast<MujocoMotor>(motors[i]);
+        if (!motor) {
+            return false;
+        }
+        std::scoped_lock lock(motor->mtx_);
+        motor->target_q_ = clamped_positions[i];
+        motor->mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
     }
     return true;
 }

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

@@ -30,7 +30,7 @@ namespace cmvr {
 
             bool initNode(uint8_t node_id) override;
 
-            void setMode(uint8_t node_id, msgs::RunMode mode) override;
+            bool 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;

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

@@ -263,7 +263,7 @@ void Ti5MotorCanopenProtocol::writeProfilePositionTargetBySdo(uint8_t node_id, i
     seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
 }
 
-void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
+bool Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
     // cur_mode_[node_id] = mode;
 
 
@@ -334,8 +334,16 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
         }
         default:
             // TODO: Handle unspecified or unknown mode
-            break;
+            return false;
     }
+
+    if (!waitUntil([&]() { return getMode(node_id) == mode; }, 500)) {
+        CMVR_LOG(ERROR) << "motor " << static_cast<int>(node_id)
+                        << ": operation mode switch failed, target_mode="
+                        << static_cast<int>(mode);
+        return false;
+    }
+    return true;
 }
 
 void Ti5MotorCanopenProtocol::seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms) {
@@ -563,8 +571,7 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) {
 }
 
 bool Ti5MotorCanopenProtocol::torqueOn(uint8_t node_id) {
-    setMode(node_id, msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
-    return true;
+    return setMode(node_id, msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
 }
 
 bool Ti5MotorCanopenProtocol::brakeRelease(uint8_t node_id) {

cmvr-es/devices/motor/manager/include/motor_manager.h

@@ -45,6 +45,14 @@ public:
     std::shared_ptr<AbstractMotor> getMotor(std::uint8_t node_id) const;
     std::shared_ptr<AbstractMotor> getMotor(const std::string& joint_name) const;
     const std::unordered_map<std::string, std::shared_ptr<AbstractMotor>>& motorsMap() const;
+    bool commandCyclicPositionsAtomic(
+        const std::vector<std::shared_ptr<AbstractMotor>>& motors,
+        const std::vector<double>& positions,
+        const std::vector<double>& velocities) const;
+    bool readFeedbacksAtomic(
+        const std::vector<std::shared_ptr<AbstractMotor>>& motors,
+        std::vector<double>& positions,
+        std::vector<double>& velocities) const;
 
     static std::shared_ptr<MotorManager> managerFor(const std::string& id);
     static std::shared_ptr<simulate::MujocoWorld> mujocoWorldFor(const std::string& id);

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

@@ -239,6 +239,46 @@ const std::unordered_map<std::string, std::shared_ptr<AbstractMotor>>& MotorMana
     return motors_by_joint_;
 }
 
+bool MotorManager::commandCyclicPositionsAtomic(
+    const std::vector<std::shared_ptr<AbstractMotor>>& motors,
+    const std::vector<double>& positions,
+    const std::vector<double>& velocities) const
+{
+    if (motors.empty() || motors.size() != positions.size() ||
+        motors.size() != velocities.size()) {
+        return false;
+    }
+
+    if (std::dynamic_pointer_cast<EyouMotor>(motors.front())) {
+        return EyouMotor::commandCyclicPositionsAtomic(motors, positions, velocities);
+    }
+    if (std::dynamic_pointer_cast<MujocoMotor>(motors.front())) {
+        return MujocoMotor::commandCyclicPositionsAtomic(motors, positions, velocities);
+    }
+
+    CMVR_LOG(ERROR) << "[MotorManager] atomic cyclic position is unsupported for motor type: "
+                    << motors.front()->typeName();
+    return false;
+}
+
+bool MotorManager::readFeedbacksAtomic(
+    const std::vector<std::shared_ptr<AbstractMotor>>& motors,
+    std::vector<double>& positions,
+    std::vector<double>& velocities) const
+{
+    if (motors.empty()) {
+        return false;
+    }
+
+    if (std::dynamic_pointer_cast<EyouMotor>(motors.front())) {
+        return EyouMotor::readFeedbacksAtomic(motors, positions, velocities);
+    }
+
+    CMVR_LOG(ERROR) << "[MotorManager] atomic feedback is unsupported for motor type: "
+                    << motors.front()->typeName();
+    return false;
+}
+
 std::shared_ptr<MotorManager> MotorManager::managerFor(const std::string& id)
 {
     std::lock_guard<std::mutex> lock(registry_mutex_);

cmvr-es/devices/motor/motor_protocol_interface.h

@@ -27,7 +27,7 @@ namespace cmvr {
              */
             virtual bool initNode(uint8_t node_id) = 0;
 
-            virtual void setMode(uint8_t node_id,msgs::RunMode mode ) = 0;
+            virtual bool 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;
             virtual void setLimitQd(uint8_t node_id,double qd) = 0;

docs/ethercat_motor_tutorial.md

@@ -75,6 +75,14 @@ motor {
         stopped_velocity_tolerance_rad_s: 0.001
       }
 
+      zero_calibration {
+        timeout_ms: 2000
+        poll_period_ms: 10
+        stable_sample_count: 5
+        position_tolerance_counts: 10000
+        stable_delta_counts: 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 }

protos/cmvr/config/motor_config/motor_config.proto

@@ -25,13 +25,20 @@ message EthercatSlaveConfig {
 }
 
 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 ZeroCalibrationConfig {
+  uint32 timeout_ms = 1;
+  uint32 poll_period_ms = 2;
+  uint32 stable_sample_count = 3;
+  uint32 position_tolerance_counts = 4;
+  uint32 stable_delta_counts = 5;
+}
+
 message EtherCATDcConfig {
   optional bool enable = 1;
   optional int32 reference_motor_id = 2;
@@ -54,6 +61,7 @@ message EtherCATConfig {
   EtherCATDcConfig dc = 4;
   optional uint32 slave_op_timeout_ms = 5;
   optional uint32 slave_state_poll_period_ms = 6;
+  ZeroCalibrationConfig zero_calibration = 7;
   repeated EthercatSlaveConfig slaves = 10;
 }
 

script/ethercat/start_ethercat.sh

@@ -20,6 +20,7 @@ Environment:
   IGH_ROOT=...                 Override bundled IgH install path.
   ETHERCAT_CONF=...            Override ethercatctl config path.
   ETHERCAT_DEV=/dev/EtherCAT0  Override EtherCAT character device node.
+  ETHERCAT_GROUP=plugdev       Group allowed to access the character device.
   START_WAIT_SEC=5             Seconds to wait for link/slave discovery.
 USAGE
 }
@@ -36,6 +37,7 @@ fi
 
 IFACE="${IFACE:-eno1}"
 ETHERCAT_DEV="${ETHERCAT_DEV:-/dev/EtherCAT0}"
+ETHERCAT_GROUP="${ETHERCAT_GROUP:-plugdev}"
 START_WAIT_SEC="${START_WAIT_SEC:-5}"
 
 NON_NUMERIC_ARG_COUNT=0
@@ -117,7 +119,8 @@ ip link set "${IFACE}" up
 "${ETHERCATCTL}" -c "${ETHERCAT_CONF}" start
 
 if [[ -e "${ETHERCAT_DEV}" ]]; then
-  chmod 666 "${ETHERCAT_DEV}"
+  chgrp "${ETHERCAT_GROUP}" "${ETHERCAT_DEV}"
+  chmod 660 "${ETHERCAT_DEV}"
 else
   echo "warning: ${ETHERCAT_DEV} not found; skip chmod. Check with: ls -l /dev/EtherCAT*" >&2
 fi