From abe69c46f018793178f62fd192517146a8de2e8f Mon Sep 17 00:00:00 2001 From: lgv Date: Thu, 30 Jul 2026 14:11:38 +0800 Subject: [PATCH] fix(eyou): make zero calibration transactional --- cmvr-es/common/math/support_functions.h | 20 ++ .../devices/motor/ethercat_motors.pb.txt | 9 +- ... => ethercat_motors_four_real_test.pb.txt} | 9 +- .../include/vendor/eyou/eyou_motor_adapter.h | 1 + .../include/vendor/motor_vendor_adapter.h | 1 + .../src/vendor/eyou/eyou_motor.cpp | 5 +- .../src/vendor/eyou/eyou_motor_adapter.cpp | 257 ++++++++++++++---- .../eyou_motor_device_manager_real_test.cpp | 2 +- .../src/vendor/eyou/eyou_motor_real_test.cpp | 10 +- docs/ethercat_motor_tutorial.md | 8 + .../config/motor_config/motor_config.proto | 10 +- 11 files changed, 274 insertions(+), 58 deletions(-) rename cmvr-es/config/devices/motor/{ethercat_motors_two_real_test.pb.txt => ethercat_motors_four_real_test.pb.txt} (90%) diff --git a/cmvr-es/common/math/support_functions.h b/cmvr-es/common/math/support_functions.h index f6c38923..2be9b19e 100644 --- a/cmvr-es/common/math/support_functions.h +++ b/cmvr-es/common/math/support_functions.h @@ -3,6 +3,7 @@ // #pragma once +#include #include #include #include @@ -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(lhs) - static_cast(rhs) + : static_cast(rhs) - static_cast(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 eigen_to_vector(const Eigen::VectorXd &v) { return std::vector(v.data(), v.data() + v.size()); } diff --git a/cmvr-es/config/devices/motor/ethercat_motors.pb.txt b/cmvr-es/config/devices/motor/ethercat_motors.pb.txt index c49936e0..9fda778a 100644 --- a/cmvr-es/config/devices/motor/ethercat_motors.pb.txt +++ b/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 diff --git a/cmvr-es/config/devices/motor/ethercat_motors_two_real_test.pb.txt b/cmvr-es/config/devices/motor/ethercat_motors_four_real_test.pb.txt similarity index 90% rename from cmvr-es/config/devices/motor/ethercat_motors_two_real_test.pb.txt rename to cmvr-es/config/devices/motor/ethercat_motors_four_real_test.pb.txt index c473c5ce..8e69905a 100644 --- a/cmvr-es/config/devices/motor/ethercat_motors_two_real_test.pb.txt +++ b/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 diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h index a9d95490..c177a313 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h +++ b/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; diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h index 5af59549..7eb65f95 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h +++ b/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; }; 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 18e4c2eb..20fcbfa2 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 @@ -90,8 +90,11 @@ bool EyouMotor::calibrateZeroQ() return false; } + const auto counts_per_joint_revolution = static_cast( + 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; } diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp index f43fee3e..04e0fcfa 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp +++ b/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( + node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003, + 0x00, original_soft_limit_state) || + !bus_runtime_->readSdo( + node_id, msgs::CIA402_HOME_OFFSET_607C, + 0x00, original_home_offset) || + !bus_runtime_->readSdo( + node_id, msgs::CIA402_ACTUAL_POSITION_6064, + 0x00, original_position)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to snapshot calibration state, node=" + << static_cast(node_id); + return false; + } + + // EYOU applies HomeOffset additively, so clearing it exposes this raw position. + const auto expected_cleared_position_wide = + static_cast(original_position) - + static_cast(original_home_offset); + if (expected_cleared_position_wide < std::numeric_limits::min() || + expected_cleared_position_wide > std::numeric_limits::max()) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] cleared position would overflow, node=" + << static_cast(node_id) + << ", original_position=" << original_position + << ", original_home_offset=" << original_home_offset; + return false; + } + const auto expected_cleared_position = + static_cast(expected_cleared_position_wide); + + const auto write_home_offset = [&](const std::int32_t value) { + return bus_runtime_->writeSdo( + node_id, msgs::CIA402_HOME_OFFSET_607C, 0x00, value); + }; + const auto save_parameters = [&]() { + return bus_runtime_->writeSdo( + 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( + 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( + 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( + node_id, msgs::CIA402_HOME_OFFSET_607C, + 0x00, observed_offset) && + bus_runtime_->readSdo( + 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(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(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(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(node_id); + << static_cast(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(node_id); + return rollback("disable_soft_limit"); + } - if (!bus_runtime_->writeSdo(node_id, msgs::CIA402_HOME_OFFSET_607C, - 0x00, 0)) { + if (!write_home_offset(0)) { CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to clear home offset, node=" << static_cast(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(node_id, msgs::CIA402_ACTUAL_POSITION_6064, - 0x00, actual_position)) { - CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read actual position " - << "after clearing home offset, node=" - << static_cast(node_id); - return false; + if (!wait_for_position("clear_home_offset", 0, + expected_cleared_position, actual_position)) { + return rollback("wait_for_cleared_position"); } - if (actual_position == std::numeric_limits::min()) { CMVR_LOG(ERROR) << "[EyouMotorAdapter] invalid actual position for home " << "offset calibration, node=" << static_cast(node_id) << ", actual_position=" << actual_position; - return false; + return rollback("negate_actual_position"); } const auto home_offset = static_cast(-actual_position); - if (!bus_runtime_->writeSdo(node_id, msgs::CIA402_HOME_OFFSET_607C, - 0x00, home_offset)) { + if (!write_home_offset(home_offset)) { CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write home offset, node=" << static_cast(node_id) << ", home_offset=" << home_offset; - return false; + return rollback("write_home_offset"); } - if (!bus_runtime_->writeSdo( - node_id, eyou::EYOU_STORE_PARAMETERS_1010, - 0x01, - 0x65766173)) { + if (!wait_for_position("apply_home_offset", home_offset, 0, + zeroed_position)) { + return rollback("wait_for_zero_before_save"); + } + + if (!save_parameters()) { CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to save home offset parameter, node=" << static_cast(node_id); - return false; + return rollback("save_parameters"); } - std::this_thread::sleep_for(std::chrono::milliseconds{50}); - std::int32_t home_offset_readback = 0; - if (!bus_runtime_->readSdo(node_id, msgs::CIA402_HOME_OFFSET_607C, - 0x00, home_offset_readback)) { - CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read back home offset, node=" - << static_cast(node_id); - return false; + if (!wait_for_position("save_home_offset", home_offset, 0, + zeroed_position)) { + return rollback("wait_for_zero_after_save"); } - if (home_offset_readback != home_offset) { - CMVR_LOG(ERROR) << "[EyouMotorAdapter] home offset readback mismatch, node=" + + if (!restore_soft_limit()) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to restore software position " + << "limit after home offset calibration, node=" << static_cast(node_id) - << ", expected=" << home_offset - << ", actual=" << home_offset_readback; - return false; - } - - if (!bus_runtime_->readSdo(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(node_id); - return false; - } - if (std::abs(static_cast(zeroed_position)) > 10000) { - CMVR_LOG(ERROR) << "[EyouMotorAdapter] home offset did not zero actual position, " - << "node=" << static_cast(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; + << ", original_soft_limit_state=" << original_soft_limit_state; + return rollback("restore_soft_limit"); } + CMVR_LOG(INFO) << "[EyouMotorAdapter] home offset calibration completed, node=" + << static_cast(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; } diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp index 3532cf73..9c2fbe9b 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp +++ b/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 kFourMotorIds{1, 2, 3, 4}; constexpr std::chrono::milliseconds kCyclicCommandPeriod{1}; constexpr std::chrono::milliseconds kPrintPeriod{100}; diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp index 80a1156c..7753fda1 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp +++ b/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); diff --git a/docs/ethercat_motor_tutorial.md b/docs/ethercat_motor_tutorial.md index 2ba49119..e1565180 100644 --- a/docs/ethercat_motor_tutorial.md +++ b/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 } diff --git a/protos/cmvr/config/motor_config/motor_config.proto b/protos/cmvr/config/motor_config/motor_config.proto index c31a4d8a..7c166df9 100644 --- a/protos/cmvr/config/motor_config/motor_config.proto +++ b/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; }