SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix(eyou): make zero calibration transactional

Browse Source
This commit is contained in:
lgv 2026-07-30 14:11:38 +08:00
parent 3ac7db50fd
commit abe69c46f0
11 changed files with 274 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
cmvr-es/common/math/support_functions.h
View File

@ -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());
}

9
cmvr-es/config/devices/motor/ethercat_motors.pb.txt
View File

@ -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

9
cmvr-es/config/devices/motor/ethercat_motors_two_real_test.pb.txt → cmvr-es/config/devices/motor/ethercat_motors_four_real_test.pb.txt
View File

@ -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

1
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h vendored
View File

@ -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;

1
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h vendored
View File

@ -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;
};

5
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp vendored
View File

@ -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;
}

255
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp vendored
View File

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

2
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp vendored
View File

@ -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};

10
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp vendored
View File

@ -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);

8
docs/ethercat_motor_tutorial.md
View File

@ -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 }

10
protos/cmvr/config/motor_config/motor_config.proto
View File

@ -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;
}