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

Merge branch 'dev' into linbo_dev

Browse Source 
# Conflicts:
#	cmvr-es/config/devices/motor/ethercat_motors.pb.txt
#	cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp
#	cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp
#	cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp
#	dependency/x86/third_party/ethercat/v1.7.0/bin/ethercat
#	dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.a
#	dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so.1.2.0
This commit is contained in:
linbo 2026-08-03 13:39:58 +08:00
commit 7fb7c4dcbe
38 changed files with 1816 additions and 333 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
cmake/FindExternalLib.cmake
View File

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

6
cmvr-es/common/CMakeLists.txt
View File

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

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

34
cmvr-es/common/math/support_functions_test.cpp Normal file
View File

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

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

@ -14,15 +14,22 @@ motor {
slave_state_poll_period_ms: 10
cia402 {
profile_position_trigger_delay_ms: 2
state_transition_timeout_ms: 5000
state_transition_timeout_ms: 1200
velocity_stop_timeout_ms: 2000
status_poll_period_ms: 10
stopped_velocity_tolerance_rad_s: 0.001
}
zero_calibration {
timeout_ms: 2000
poll_period_ms: 10
stable_sample_count: 5
position_tolerance_counts: 10000
stable_delta_counts: 1000
}
dc {
enable: false
enable: true
reference_motor_id: 1
sync0_cycle_us: 1000
sync0_shift_us: 0
@ -32,6 +39,12 @@ motor {
}
slaves { motor_id: 1 alias: 0 position: 0 }
slaves { motor_id: 2 alias: 0 position: 1 }
slaves { motor_id: 3 alias: 0 position: 2 }
slaves { motor_id: 4 alias: 0 position: 3 }
slaves { motor_id: 5 alias: 0 position: 4 }
slaves { motor_id: 6 alias: 0 position: 5 }
slaves { motor_id: 7 alias: 0 position: 6 }
}
joint_limits {
@ -48,6 +61,12 @@ motor {
motors {
motors { id: 1 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 2 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 3 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 4 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 5 joint_name: "R_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 6 joint_name: "R_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 7 joint_name: "R_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
}
}
}

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

25
cmvr-es/devices/arm/aubo_arm/CMakeLists.txt
View File

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

50
cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp
View File

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

6
cmvr-es/devices/motor/abstract_motor.h
View File

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

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

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

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

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

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

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

1
cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt
View File

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

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

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

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

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

78
cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_status_monitor.h Normal file
View File

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

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

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

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

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

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

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

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

284
cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_status_monitor.cpp Normal file
View File

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

8
cmvr-es/devices/motor/manager/include/motor_manager.h
View File

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

40
cmvr-es/devices/motor/manager/src/motor_manager.cpp
View File

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

2
cmvr-es/devices/motor/motor_protocol_interface.h
View File

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

BIN
dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-136-generic/ethercat/devices/ec_generic.ko vendored Normal file
View File

Binary file not shown.

BIN
dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-136-generic/ethercat/examples/mini/ec_mini.ko vendored Normal file
View File

Binary file not shown.

BIN
dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-136-generic/ethercat/master/ec_master.ko vendored Normal file
View File

Binary file not shown.

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

5
script/ethercat/start_ethercat.sh
View File

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