diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp
index 5a5db3a3..26f22d14 100644
--- a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp
+++ b/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)) {
-                return Result::failure(ArmErrorCode::CommandFailed,
-                                       "failed to command cyclic position for joint: " +
-                                           motors[i]->jointName());
-            }
+        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 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,13 +501,18 @@ 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)) {
-            return Result::failure(ArmErrorCode::CommandFailed,
-                                   "failed to command cyclic position for joint: " +
-                                       joint_names_[i]);
+            if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
+                return Result::failure(ArmErrorCode::CommandFailed,
+                                       "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,10 +796,8 @@ 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])) {
-                return false;
-            }
+        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));
diff --git a/cmvr-es/devices/motor/abstract_motor.h b/cmvr-es/devices/motor/abstract_motor.h
index 35eee16d..28a7d288 100644
--- a/cmvr-es/devices/motor/abstract_motor.h
+++ b/cmvr-es/devices/motor/abstract_motor.h
@@ -48,13 +48,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() {
diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h b/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h
index 5b2c7607..02150507 100644
--- a/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h
+++ b/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h
@@ -3,6 +3,7 @@
 
 #include <atomic>
 #include <chrono>
+#include <cstddef>
 #include <cstdint>
 #include <cstring>
 #include <mutex>
@@ -23,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;
@@ -44,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
     {
@@ -87,10 +135,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_();
@@ -159,6 +228,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};
 };
diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp
index 4b5554c5..d6148be4 100644
--- a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp
+++ b/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);
-    if (slave_it == slaves_by_motor_id_.end()) {
-        return false;
+
+    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;
+        }
+        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;
+        }
+        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;
+            }
+        }
     }
-    auto entry_it = slave_it->second.pdo_entries.find(pdoEntryKey_(index, subindex));
-    if (entry_it == slave_it->second.pdo_entries.end()) {
-        return false;
-    }
-    auto& entry = entry_it->second;
-    if (!entry.rx || entry.cfg.bit_len != bit_len) {
-        return false;
-    }
-    entry.value = maskValue_(value, entry.cfg.bit_len);
-    if (domain_data_) {
-        writeEntryValue_(domain_data_, entry);
+
+    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);
-    if (slave_it == slaves_by_motor_id_.end()) {
-        return false;
+    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_(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_it = slave_it->second.pdo_entries.find(pdoEntryKey_(index, subindex));
-    if (entry_it == slave_it->second.pdo_entries.end()) {
-        return false;
+
+    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);
     }
-    const auto& entry = entry_it->second;
-    if (entry.rx || entry.cfg.bit_len != bit_len) {
-        return false;
-    }
-    value = maskValue_(entry.value, entry.cfg.bit_len);
     return true;
 }
 
diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt b/cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt
index 8c7d93df..36685aeb 100644
--- a/cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt
+++ b/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
 )
diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h
index e3e92dcb..62f869f4 100644
--- a/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h
+++ b/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
diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h
index c4cc46b5..f0903a91 100644
--- a/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h
+++ b/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
diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_status_monitor.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_status_monitor.h
new file mode 100644
index 00000000..3a92a7df
--- /dev/null
+++ b/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
diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h
index 10f3ee96..4bdc2154 100644
--- a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h
+++ b/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"
@@ -24,6 +25,15 @@ public:
     bool calibrateZeroQ() override;
     bool brakeRelease() 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;
diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp
index 7dbd8e65..a621ddda 100644
--- a/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp
+++ b/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");
 
-    if (!writeControlwordAndWait_(node_id, cia402::switchOnControlword(),
-                                  cia402::DeviceState::SwitchedOn,
-                                  "Switched On")) {
+    std::uint16_t statusword = 0;
+    if (!readStatusword_(node_id, statusword)) {
         return false;
     }
-    if (!writeControlwordAndWait_(node_id, cia402::shutdownControlword(),
-                                  cia402::DeviceState::ReadyToSwitchOn,
-                                  "Ready To Switch On")) {
-        return false;
+    const auto status = cia402::statusword(statusword);
+    if (cia402::hasState(status, cia402::DeviceState::SwitchOnDisabled) ||
+        cia402::hasState(status, cia402::DeviceState::ReadyToSwitchOn)) {
+        return true;
     }
 
-    return true;
+    if (cia402::hasState(status, cia402::DeviceState::OperationEnabled)) {
+        if (!writeControlwordAndWait_(node_id, cia402::switchOnControlword(),
+                                      cia402::DeviceState::SwitchedOn,
+                                      "Switched On")) {
+            return false;
+        }
+        return writeControlwordAndWait_(node_id, cia402::shutdownControlword(),
+                                        cia402::DeviceState::ReadyToSwitchOn,
+                                        "Ready To Switch On");
+    }
+
+    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,
-                                                         NodeState& state)
+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);
-    writeControlword_(node_id, state.controlword);
+    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,
-                                                  const msgs::RunMode mode,
-                                                  const NodeState& state) const
+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
diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_status_monitor.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_status_monitor.cpp
new file mode 100644
index 00000000..1e0eeb5f
--- /dev/null
+++ b/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
diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp
index 73ae01ef..18e4c2eb 100644
--- a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp
+++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp
@@ -1,7 +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 <utility>
 
 #include "common/base/logging/logger.h"
@@ -100,6 +104,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_);
diff --git a/cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h b/cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h
index 5eb7b5e3..57c97387 100644
--- a/cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h
+++ b/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,9 +45,10 @@ public:
     double getQ() override;
     double getQd() override;
 
-    static bool setTargetsAtomic(const std::vector<std::shared_ptr<MujocoMotor>>& motors,
-                                 const std::vector<double>& positions,
-                                 const std::vector<double>& velocities);
+    static bool commandCyclicPositionsAtomic(
+        const std::vector<std::shared_ptr<AbstractMotor>>& motors,
+        const std::vector<double>& positions,
+        const std::vector<double>& velocities);
 
 private:
     bool holdPosition_();
diff --git a/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp b/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp
index 07b0bda4..447e8d18 100644
--- a/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp
+++ b/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,9 +217,10 @@ double MujocoMotor::getQd()
     return qd;
 }
 
-bool MujocoMotor::setTargetsAtomic(const std::vector<std::shared_ptr<MujocoMotor>>& motors,
-                                   const std::vector<double>& positions,
-                                   const std::vector<double>& velocities)
+bool MujocoMotor::commandCyclicPositionsAtomic(
+    const std::vector<std::shared_ptr<AbstractMotor>>& motors,
+    const std::vector<double>& positions,
+    const std::vector<double>& velocities)
 {
     if (motors.size() != positions.size() || motors.size() != velocities.size() || motors.empty()) {
         return false;
@@ -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;
 }
diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h
index 0588fd3f..3b64a284 100644
--- a/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h
+++ b/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;
diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp b/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp
index 69e1f562..14937d69 100644
--- a/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp
+++ b/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) {
diff --git a/cmvr-es/devices/motor/manager/include/motor_manager.h b/cmvr-es/devices/motor/manager/include/motor_manager.h
index 3bbd2cab..3673355a 100644
--- a/cmvr-es/devices/motor/manager/include/motor_manager.h
+++ b/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);
diff --git a/cmvr-es/devices/motor/manager/src/motor_manager.cpp b/cmvr-es/devices/motor/manager/src/motor_manager.cpp
index 5e83b043..3a6b5118 100644
--- a/cmvr-es/devices/motor/manager/src/motor_manager.cpp
+++ b/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_);
diff --git a/cmvr-es/devices/motor/motor_protocol_interface.h b/cmvr-es/devices/motor/motor_protocol_interface.h
index 4b8f3e34..6afbcb64 100644
--- a/cmvr-es/devices/motor/motor_protocol_interface.h
+++ b/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;