fix(eyou): make zero calibration transactional

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/config/devices/motor/ethercat_motors.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: true
         reference_motor_id: 1

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/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h

@@ -21,6 +21,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;
 

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/vendor/eyou/eyou_motor.cpp

@@ -90,8 +90,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;
     }

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,
+    if (!write_home_offset(0)) {
-                                              0x00, 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,
+    if (!wait_for_position("clear_home_offset", 0,
-                                             0x00, actual_position)) {
+                           expected_cleared_position, actual_position)) {
-        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read actual position "
+        return rollback("wait_for_cleared_position");
-                        << "after clearing home offset, node="
-                        << static_cast<int>(node_id);
-        return false;
     }
-
     if (actual_position == std::numeric_limits<std::int32_t>::min()) {
         CMVR_LOG(ERROR) << "[EyouMotorAdapter] invalid actual position for home "
                         << "offset calibration, node=" << static_cast<int>(node_id)
                         << ", actual_position=" << actual_position;
-        return false;
+        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,
+    if (!write_home_offset(home_offset)) {
-                                              0x00, 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>(
+    if (!wait_for_position("apply_home_offset", home_offset, 0,
-            node_id, eyou::EYOU_STORE_PARAMETERS_1010,
+                           zeroed_position)) {
-            0x01,
+        return rollback("wait_for_zero_before_save");
-            0x65766173)) {
+    }
+
+    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 (!wait_for_position("save_home_offset", home_offset, 0,
-    if (!bus_runtime_->readSdo<std::int32_t>(node_id, msgs::CIA402_HOME_OFFSET_607C,
+                           zeroed_position)) {
-                                             0x00, home_offset_readback)) {
+        return rollback("wait_for_zero_after_save");
-        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read back home offset, node="
-                        << static_cast<int>(node_id);
-        return false;
     }
-    if (home_offset_readback != home_offset) {
+
-        CMVR_LOG(ERROR) << "[EyouMotorAdapter] home offset readback mismatch, node="
+    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
+                        << ", original_soft_limit_state=" << original_soft_limit_state;
-                        << ", actual=" << home_offset_readback;
+        return rollback("restore_soft_limit");
-        return false;
-    }
-
-    if (!bus_runtime_->readSdo<std::int32_t>(node_id, msgs::CIA402_ACTUAL_POSITION_6064,
-                                             0x00, zeroed_position)) {
-        CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read actual position "
-                        << "after writing home offset, node="
-                        << static_cast<int>(node_id);
-        return false;
-    }
-    if (std::abs(static_cast<long long>(zeroed_position)) > 10000) {
-        CMVR_LOG(ERROR) << "[EyouMotorAdapter] home offset did not zero actual position, "
-                        << "node=" << static_cast<int>(node_id)
-                        << ", actual_position_before=" << actual_position
-                        << ", home_offset=" << home_offset
-                        << ", home_offset_readback=" << home_offset_readback
-                        << ", actual_position_after=" << zeroed_position
-                        << ", tolerance_counts=" << 10000;
-        return false;
     }
 
+    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
+                   << ", 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

@@ -21,7 +21,7 @@ namespace {
 
 constexpr const char* kMotorManagerId = "ethercat_motors";
 constexpr const char* kMotorConfigFile =
-    "devices/motor/ethercat_motors_two_real_test.pb.txt";
+    "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};

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

@@ -46,14 +46,20 @@ 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(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(true);
+    dc->set_enable(false);
     dc->set_reference_motor_id(kMotorId);
     dc->set_sync0_cycle_us(1000);
     dc->set_sync0_shift_us(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;
 }