diff --git a/cmake/FindExternalLib.cmake b/cmake/FindExternalLib.cmake index 31ee4f6c..44a25ea7 100644 --- a/cmake/FindExternalLib.cmake +++ b/cmake/FindExternalLib.cmake @@ -55,7 +55,17 @@ function(setup_external_libs ARCH) # ---- library dirs ---- if(EXISTS "${FULL_PATH}/lib") - list(APPEND LIBRARY_DIRS "${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.* diff --git a/cmvr-es/common/CMakeLists.txt b/cmvr-es/common/CMakeLists.txt index 3bedb230..a9e3f852 100644 --- a/cmvr-es/common/CMakeLists.txt +++ b/cmvr-es/common/CMakeLists.txt @@ -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 #) diff --git a/cmvr-es/common/math/support_functions.h b/cmvr-es/common/math/support_functions.h index f6c38923..2be9b19e 100644 --- a/cmvr-es/common/math/support_functions.h +++ b/cmvr-es/common/math/support_functions.h @@ -3,6 +3,7 @@ // #pragma once +#include #include #include #include @@ -14,6 +15,25 @@ class SupportFunctions { private: static constexpr double EPS = 1e-9; public: + static constexpr std::int64_t absoluteDifference(const std::int32_t lhs, + const std::int32_t rhs) noexcept { + return lhs >= rhs + ? static_cast(lhs) - static_cast(rhs) + : static_cast(rhs) - static_cast(lhs); + } + + static constexpr std::int64_t cyclicAbsoluteDifference( + const std::int32_t lhs, + const std::int32_t rhs, + const std::int64_t period) noexcept { + const auto linear_distance = absoluteDifference(lhs, rhs); + if (period <= 0) { + return linear_distance; + } + const auto wrapped_distance = linear_distance % period; + return std::min(wrapped_distance, period - wrapped_distance); + } + static std::vector eigen_to_vector(const Eigen::VectorXd &v) { return std::vector(v.data(), v.data() + v.size()); } diff --git a/cmvr-es/common/math/support_functions_test.cpp b/cmvr-es/common/math/support_functions_test.cpp new file mode 100644 index 00000000..cf1a7c4e --- /dev/null +++ b/cmvr-es/common/math/support_functions_test.cpp @@ -0,0 +1,34 @@ +#include +#include + +#include + +#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::min(), + std::numeric_limits::max(), period); + EXPECT_GE(distance, 0); + EXPECT_LE(distance, period / 2); +} diff --git a/cmvr-es/config/devices/motor/ethercat_motors.pb.txt b/cmvr-es/config/devices/motor/ethercat_motors.pb.txt index f1371099..9fda778a 100644 --- a/cmvr-es/config/devices/motor/ethercat_motors.pb.txt +++ b/cmvr-es/config/devices/motor/ethercat_motors.pb.txt @@ -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 } } } } diff --git a/cmvr-es/config/devices/motor/ethercat_motors_two_real_test.pb.txt b/cmvr-es/config/devices/motor/ethercat_motors_four_real_test.pb.txt similarity index 91% rename from cmvr-es/config/devices/motor/ethercat_motors_two_real_test.pb.txt rename to cmvr-es/config/devices/motor/ethercat_motors_four_real_test.pb.txt index 01ca859a..85c114bd 100644 --- a/cmvr-es/config/devices/motor/ethercat_motors_two_real_test.pb.txt +++ b/cmvr-es/config/devices/motor/ethercat_motors_four_real_test.pb.txt @@ -14,13 +14,20 @@ motor { slave_state_poll_period_ms: 10 cia402 { - profile_position_trigger_delay_ms: 2 state_transition_timeout_ms: 1200 velocity_stop_timeout_ms: 2000 status_poll_period_ms: 10 stopped_velocity_tolerance_rad_s: 0.001 } + zero_calibration { + timeout_ms: 2000 + poll_period_ms: 10 + stable_sample_count: 5 + position_tolerance_counts: 10000 + stable_delta_counts: 1000 + } + dc { enable: false reference_motor_id: 1 diff --git a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt index b8947772..d56d09cb 100644 --- a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt @@ -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) diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp index 5a5db3a3..26f22d14 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp @@ -1,5 +1,6 @@ #include "arm/motor_robot_arm/include/motor_robot_arm.h" +#include #include #include #include @@ -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 command_velocity(motors.size(), 0.0); for (std::size_t k = 1; k < samples.size(); ++k) { const auto& sample = samples[k]; if (sample.position.size() != motors.size()) { return Result::failure(ArmErrorCode::CommandFailed, "moveJ sample size mismatch"); } - for (std::size_t i = 0; i < motors.size(); ++i) { - const double qd = i < sample.velocity.size() ? sample.velocity[i] : 0.0; - if (!motors[i]->commandCyclicPosition(sample.position[i], qd)) { - return Result::failure(ArmErrorCode::CommandFailed, - "failed to command cyclic position for joint: " + - motors[i]->jointName()); - } + std::fill(command_velocity.begin(), command_velocity.end(), 0.0); + std::copy_n(sample.velocity.begin(), + std::min(sample.velocity.size(), command_velocity.size()), + command_velocity.begin()); + if (!motor_manager_->commandCyclicPositionsAtomic( + motors, sample.position, command_velocity)) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to submit atomic cyclic position command"); } if (k + 1 < samples.size()) { const double next_t = samples[k + 1].t > 0.0 ? samples[k + 1].t @@ -358,7 +365,11 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity, "motor not found for joint: " + joint_names_[i]); } if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) { - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); + if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY)) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to set cyclic velocity mode for joint: " + + joint_names_[i]); + } } if (!motor->commandCyclicVelocity(velocity.velocity[i] * speed_scaling_)) { return Result::failure(ArmErrorCode::CommandFailed, @@ -481,6 +492,8 @@ Result MotorRobotArm::servoJ(const JointPositionCommand& target) } std::lock_guard lock(mutex_); + std::vector> motors; + motors.reserve(joint_names_.size()); for (std::size_t i = 0; i < joint_names_.size(); ++i) { auto motor = getMotor_(joint_names_[i]); if (!motor) { @@ -488,13 +501,18 @@ Result MotorRobotArm::servoJ(const JointPositionCommand& target) "motor not found for joint: " + joint_names_[i]); } if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - } - if (!motor->commandCyclicPosition(target.position[i], 0.0)) { - return Result::failure(ArmErrorCode::CommandFailed, - "failed to command cyclic position for joint: " + - joint_names_[i]); + if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to set cyclic position mode for joint: " + + joint_names_[i]); + } } + motors.push_back(std::move(motor)); + } + const std::vector velocities(motors.size(), 0.0); + if (!motor_manager_->commandCyclicPositionsAtomic(motors, target.position, velocities)) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to submit atomic cyclic position command"); } return Result::success(); } @@ -762,7 +780,9 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj return false; } if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) { + return false; + } } motors.push_back(std::move(motor)); } @@ -776,10 +796,8 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj if (position.size() != motors.size() || velocity.size() != motors.size()) { return false; } - for (std::size_t j = 0; j < motors.size(); ++j) { - if (!motors[j]->commandCyclicPosition(position[j], velocity[j])) { - return false; - } + if (!motor_manager_->commandCyclicPositionsAtomic(motors, position, velocity)) { + return false; } next_deadline += std::chrono::duration_cast( std::chrono::duration(dt_segment)); diff --git a/cmvr-es/devices/motor/abstract_motor.h b/cmvr-es/devices/motor/abstract_motor.h index 869551e5..d0d224e0 100644 --- a/cmvr-es/devices/motor/abstract_motor.h +++ b/cmvr-es/devices/motor/abstract_motor.h @@ -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() { diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h b/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h index 8a4bbae8..21727952 100644 --- a/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h +++ b/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h @@ -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 + static PdoWrite makePdoWrite(int motor_id, + std::uint16_t index, + std::uint8_t subindex, + T value) + { + return PdoWrite{motor_id, index, subindex, valueBitLength_(), toRawValue_(value)}; + } + + bool writePdosAtomic(const PdoWrite* writes, std::size_t count); + + template + static PdoRead makePdoRead(int motor_id, + std::uint16_t index, + std::uint8_t subindex) + { + return PdoRead{motor_id, index, subindex, valueBitLength_(), 0}; + } + + template + static T pdoReadValue(const PdoRead& read) + { + return fromRawValue_(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 bool readPdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T& value) const { @@ -94,10 +141,31 @@ private: std::unordered_map 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 running_{false}; + std::atomic healthy_{false}; + std::atomic health_monitor_enabled_{false}; + std::atomic command_generation_{0}; + std::atomic sent_command_generation_{0}; + BusHealthState last_bus_health_; + std::unordered_map last_slave_health_; bool initialized_{false}; bool started_{false}; }; diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp index c2a9a611..d29e64b6 100644 --- a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp +++ b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp @@ -145,6 +145,10 @@ bool EthercatMotorBusRuntime::start() } running_.store(true); + healthy_.store(false); + health_monitor_enabled_.store(false); + last_bus_health_ = {}; + last_slave_health_.clear(); cyclic_thread_ = std::thread(&EthercatMotorBusRuntime::cyclicLoop_, this); started_ = true; @@ -152,6 +156,7 @@ bool EthercatMotorBusRuntime::start() stop(); return false; } + health_monitor_enabled_.store(true); CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] started EtherCAT runtime: " << id_; return true; @@ -160,6 +165,8 @@ bool EthercatMotorBusRuntime::start() void EthercatMotorBusRuntime::stop() { running_.store(false); + healthy_.store(false); + health_monitor_enabled_.store(false); if (cyclic_thread_.joinable()) { cyclic_thread_.join(); } @@ -484,6 +491,7 @@ bool EthercatMotorBusRuntime::waitSlavesOperational_() last_domain_state.wc_state == EC_WC_COMPLETE; if (all_slaves_operational && domain_complete) { + healthy_.store(true); CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] all EtherCAT slaves operational: " << id_ << ", working_counter=" << last_domain_state.working_counter; @@ -527,6 +535,8 @@ void EthercatMotorBusRuntime::cyclicLoop_() bool dc_monitor_queued = false; auto cycle_time = std::chrono::steady_clock::now(); auto next_dc_monitor_time = cycle_time + dc_monitor_period; + const auto bus_monitor_period = std::chrono::milliseconds(100); + auto next_bus_monitor_time = cycle_time; while (running_.load()) { if (dc_enabled) { ecrt_master_application_time(master_, timePointNs_(cycle_time)); @@ -551,6 +561,7 @@ void EthercatMotorBusRuntime::cyclicLoop_() std::lock_guard 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(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(master_state.al_states), + master_state.link_up != 0, + domain_result, + domain_state.working_counter, + static_cast(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 lock(data_mutex_); - auto slave_it = slaves_by_motor_id_.find(motor_id); - if (slave_it == slaves_by_motor_id_.end()) { - return false; + + for (std::size_t i = 0; i < count; ++i) { + const auto& write = writes[i]; + if (!isSupportedBitLength_(write.bit_length)) { + return false; + } + const auto slave_it = slaves_by_motor_id_.find(write.motor_id); + if (slave_it == slaves_by_motor_id_.end()) { + return false; + } + const auto entry_it = slave_it->second.pdo_entries.find( + pdoEntryKey_(write.index, write.subindex)); + if (entry_it == slave_it->second.pdo_entries.end() || + !entry_it->second.rx || + entry_it->second.cfg.bit_len != write.bit_length) { + return false; + } + for (std::size_t previous = 0; previous < i; ++previous) { + if (writes[previous].motor_id == write.motor_id && + writes[previous].index == write.index && + writes[previous].subindex == write.subindex) { + return false; + } + } } - auto entry_it = slave_it->second.pdo_entries.find(pdoEntryKey_(index, subindex)); - if (entry_it == slave_it->second.pdo_entries.end()) { - return false; - } - auto& entry = entry_it->second; - if (!entry.rx || entry.cfg.bit_len != bit_len) { - return false; - } - entry.value = maskValue_(value, entry.cfg.bit_len); - if (domain_data_) { - writeEntryValue_(domain_data_, entry); + + for (std::size_t i = 0; i < count; ++i) { + const auto& write = writes[i]; + auto& entry = slaves_by_motor_id_.at(write.motor_id) + .pdo_entries.at(pdoEntryKey_(write.index, write.subindex)); + entry.value = maskValue_(write.raw_value, entry.cfg.bit_len); } + command_generation_.fetch_add(1); return true; } @@ -668,20 +805,46 @@ bool EthercatMotorBusRuntime::readPdoRaw_(const int motor_id, const std::uint8_t bit_len, std::uint64_t& value) const { + PdoRead read{motor_id, index, subindex, bit_len, 0}; + if (!readPdosAtomic(&read, 1)) { + return false; + } + value = read.raw_value; + return true; +} + +bool EthercatMotorBusRuntime::readPdosAtomic(PdoRead* reads, + const std::size_t count) const +{ + if (!reads || count == 0) { + return false; + } + std::lock_guard lock(data_mutex_); - const auto slave_it = slaves_by_motor_id_.find(motor_id); - if (slave_it == slaves_by_motor_id_.end()) { - return false; + for (std::size_t i = 0; i < count; ++i) { + const auto& read = reads[i]; + if (!isSupportedBitLength_(read.bit_length)) { + return false; + } + const auto slave_it = slaves_by_motor_id_.find(read.motor_id); + if (slave_it == slaves_by_motor_id_.end()) { + return false; + } + const auto entry_it = slave_it->second.pdo_entries.find( + pdoEntryKey_(read.index, read.subindex)); + if (entry_it == slave_it->second.pdo_entries.end() || + entry_it->second.rx || + entry_it->second.cfg.bit_len != read.bit_length) { + return false; + } } - const auto entry_it = slave_it->second.pdo_entries.find(pdoEntryKey_(index, subindex)); - if (entry_it == slave_it->second.pdo_entries.end()) { - return false; + + for (std::size_t i = 0; i < count; ++i) { + auto& read = reads[i]; + const auto& entry = slaves_by_motor_id_.at(read.motor_id) + .pdo_entries.at(pdoEntryKey_(read.index, read.subindex)); + read.raw_value = maskValue_(entry.value, entry.cfg.bit_len); } - const auto& entry = entry_it->second; - if (entry.rx || entry.cfg.bit_len != bit_len) { - return false; - } - value = maskValue_(entry.value, entry.cfg.bit_len); return true; } diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp index a0558ce9..9f618d9a 100644 --- a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp +++ b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp @@ -1,6 +1,7 @@ #include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" #include +#include #include #include #include @@ -74,17 +75,42 @@ TEST(EthercatMotorBusRuntimeRealTest, InitStartAndReadStatusword) return; } - EXPECT_TRUE(runtime.writePdo(1, msgs::CIA402_CONTROL_WORD_6040, 0x00, 0x0000)); - EXPECT_TRUE(runtime.writePdo(1, msgs::CIA402_OPERATION_MODE_6060, 0x00, 0)); + const std::array command_writes{ + EthercatMotorBusRuntime::makePdoWrite( + 1, msgs::CIA402_CONTROL_WORD_6040, 0x00, 0x0000), + EthercatMotorBusRuntime::makePdoWrite( + 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(1, msgs::CIA402_STATUS_WORD_6041, 0x00, statusword)); + std::array feedback_reads{ + EthercatMotorBusRuntime::makePdoRead( + 1, msgs::CIA402_STATUS_WORD_6041, 0x00), + EthercatMotorBusRuntime::makePdoRead( + 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(1, msgs::CIA402_MODE_DISPLAY_6061, 0x00, mode_display)); + const auto statusword = + EthercatMotorBusRuntime::pdoReadValue(feedback_reads[0]); + const auto mode_display = + EthercatMotorBusRuntime::pdoReadValue(feedback_reads[1]); std::int16_t actual_current = 0; EXPECT_TRUE(runtime.readPdo( diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt b/cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt index 8c7d93df..36685aeb 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt @@ -1,5 +1,6 @@ add_library(ethercat_motor_driver SHARED src/cia402/cia402_protocol.cpp + src/cia402/cia402_status_monitor.cpp src/vendor/eyou/eyou_motor.cpp src/vendor/eyou/eyou_motor_adapter.cpp ) diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h index e3e92dcb..62f869f4 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h @@ -156,6 +156,11 @@ inline bool targetReached(const Statusword status) return status.target_reached != 0; } +inline bool setPointAcknowledged(const Statusword status) +{ + return (status.value & (1U << 12U)) != 0; +} + } // namespace cmvr::device::cia402 #endif // CMVR_ES_CIA402_OBJECTS_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h index c4cc46b5..f0903a91 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h @@ -3,7 +3,9 @@ #include #include +#include #include +#include #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 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 bus_runtime_; + std::unique_ptr status_monitor_; config::Cia402ProtocolConfig config_; std::unordered_map nodes_; + std::mutex cyclic_position_mutex_; + std::vector cyclic_position_writes_; }; } // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_status_monitor.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_status_monitor.h new file mode 100644 index 00000000..3a92a7df --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_status_monitor.h @@ -0,0 +1,78 @@ +#ifndef CMVR_ES_CIA402_STATUS_MONITOR_H +#define CMVR_ES_CIA402_STATUS_MONITOR_H + +#include +#include +#include +#include +#include +#include +#include + +#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" + +namespace cmvr::device { + +class Cia402StatusMonitor final { +public: + Cia402StatusMonitor(std::shared_ptr 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 bus_runtime_; + std::chrono::milliseconds poll_period_; + std::mutex monitor_mutex_; + mutable std::mutex states_mutex_; + std::unordered_map states_; + std::atomic running_{true}; + std::thread monitor_thread_; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_CIA402_STATUS_MONITOR_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h index 65a5c0e1..d4247105 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h @@ -3,6 +3,7 @@ #include #include +#include #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>& motors, + const std::vector& positions, + const std::vector& velocities); + static bool readFeedbacksAtomic( + const std::vector>& motors, + std::vector& positions, + std::vector& velocities); + private: bool hasDependencies_() const; bool hasValidConversion_() const; diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h index 80104122..cf85e802 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h @@ -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, diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h index 5af59549..7eb65f95 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h @@ -16,6 +16,7 @@ public: virtual bool writeVelocityLimit(std::uint8_t node_id, std::uint32_t velocity_limit) = 0; virtual bool calibrateZero(std::uint8_t node_id, + std::int64_t counts_per_joint_revolution, std::int32_t& zeroed_position) = 0; virtual bool brakeRelease(std::uint8_t node_id) = 0; }; diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp index 7dbd8e65..a621ddda 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp @@ -1,6 +1,7 @@ #include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h" #include +#include #include #include #include @@ -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 bus_runt config_(config) { comm_proto = CommProto::ETHERCAT; + cyclic_position_writes_.reserve(512); + status_monitor_ = std::make_unique( + 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 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( + command.node_id, + msgs::CIA402_TARGET_POSITION_607A, + 0x00, + radToCounts_(command.target_q, *state))); + cyclic_position_writes_.push_back( + EthercatMotorBusRuntime::makePdoWrite( + 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 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( + feedbacks[i].node_id, msgs::CIA402_ACTUAL_POSITION_6064, 0x00); + reads[2 * i + 1] = EthercatMotorBusRuntime::makePdoRead( + 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(reads[2 * i]); + const auto velocity = + EthercatMotorBusRuntime::pdoReadValue(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(node_id) + << ", mode=" << static_cast(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(node_id, msgs::CIA402_OPERATION_MODE_6060, 0x00, toCia402Mode_(state.mode))) { return false; @@ -238,22 +345,22 @@ bool Cia402Protocol::torqueOn(const std::uint8_t node_id) "Operation Enabled")) { return false; } + if (!waitMode_(node_id, toCia402Mode_(state.mode))) { + return false; + } std::int32_t actual_position = 0; if (readActualPosition_(node_id, actual_position)) { state.target_position = actual_position; } - writeTargetsForMode_(node_id, state.mode, state); - - state.controlword = cia402::profilePositionControlword(true); - if (!writeControlword_(node_id, state.controlword)) { - return false; - } - std::this_thread::sleep_for( - std::chrono::milliseconds(config_.profile_position_trigger_delay_ms())); - state.controlword = cia402::profilePositionControlword(false); - return writeControlword_(node_id, state.controlword); + const bool enabled = writeTargetsForMode_(node_id, state.mode, state) && + writeControlword_(node_id, state.controlword) && + waitSetPointAcknowledged_(node_id, false); + if (enabled) { + status_monitor_->setExpectedOperationEnabled(node_id, true); + } + return enabled; } bool Cia402Protocol::torqueOff(const std::uint8_t node_id) @@ -262,6 +369,7 @@ bool Cia402Protocol::torqueOff(const std::uint8_t node_id) return false; } + status_monitor_->setExpectedOperationEnabled(node_id, false); auto& state = nodeState_(node_id); std::int32_t actual_position = 0; if (readActualPosition_(node_id, actual_position)) { @@ -269,21 +377,41 @@ bool Cia402Protocol::torqueOff(const std::uint8_t node_id) } state.target_velocity = 0; state.target_torque = 0; - writeTargetsForMode_(node_id, state.mode, state); + if (!writeTargetsForMode_(node_id, state.mode, state)) { + return false; + } waitVelocityNearZero_(node_id, "torqueOff"); - if (!writeControlwordAndWait_(node_id, cia402::switchOnControlword(), - cia402::DeviceState::SwitchedOn, - "Switched On")) { + std::uint16_t statusword = 0; + if (!readStatusword_(node_id, statusword)) { return false; } - if (!writeControlwordAndWait_(node_id, cia402::shutdownControlword(), - cia402::DeviceState::ReadyToSwitchOn, - "Ready To Switch On")) { - return false; + const auto status = cia402::statusword(statusword); + if (cia402::hasState(status, cia402::DeviceState::SwitchOnDisabled) || + cia402::hasState(status, cia402::DeviceState::ReadyToSwitchOn)) { + return true; } - return true; + if (cia402::hasState(status, cia402::DeviceState::OperationEnabled)) { + if (!writeControlwordAndWait_(node_id, cia402::switchOnControlword(), + cia402::DeviceState::SwitchedOn, + "Switched On")) { + return false; + } + return writeControlwordAndWait_(node_id, cia402::shutdownControlword(), + cia402::DeviceState::ReadyToSwitchOn, + "Ready To Switch On"); + } + + if (cia402::hasState(status, cia402::DeviceState::SwitchedOn)) { + return writeControlwordAndWait_(node_id, cia402::shutdownControlword(), + cia402::DeviceState::ReadyToSwitchOn, + "Ready To Switch On"); + } + + return writeControlwordAndWait_(node_id, cia402::controlword(0), + cia402::DeviceState::SwitchOnDisabled, + "Switch On Disabled"); } bool Cia402Protocol::brakeRelease(const std::uint8_t node_id) @@ -299,10 +427,13 @@ bool Cia402Protocol::quickStop(const std::uint8_t node_id) return false; } + status_monitor_->setExpectedOperationEnabled(node_id, false); auto& state = nodeState_(node_id); state.target_velocity = 0; state.target_torque = 0; - writeTargetsForMode_(node_id, state.mode, state); + if (!writeTargetsForMode_(node_id, state.mode, state)) { + return false; + } state.controlword = cia402::quickStopControlword(); if (!writeControlword_(node_id, state.controlword)) { @@ -350,8 +481,7 @@ bool Cia402Protocol::syncTargetToActualPosition(const std::uint8_t node_id) state.target_position = actual_position; state.target_velocity = 0; state.target_torque = 0; - writeTargetsForMode_(node_id, state.mode, state); - return true; + return writeTargetsForMode_(node_id, state.mode, state); } std::int8_t Cia402Protocol::toCia402Mode_(const msgs::RunMode mode) @@ -611,6 +741,50 @@ bool Cia402Protocol::waitStatus_(const std::uint8_t node_id, return false; } +bool Cia402Protocol::waitMode_(const std::uint8_t node_id, + const std::int8_t target_mode) const +{ + const auto deadline = std::chrono::steady_clock::now() + + std::chrono::milliseconds(config_.state_transition_timeout_ms()); + std::int8_t last_mode = 0; + do { + if (readModeDisplay_(node_id, last_mode) && last_mode == target_mode) { + return true; + } + std::this_thread::sleep_for( + std::chrono::milliseconds(config_.status_poll_period_ms())); + } while (std::chrono::steady_clock::now() < deadline); + + CMVR_LOG(ERROR) << "[Cia402Protocol] timeout waiting for operation mode, node=" + << static_cast(node_id) + << ", target_mode=" << static_cast(target_mode) + << ", last_mode=" << static_cast(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(node_id) + << ", last_statusword=0x" << std::hex << last_statusword << std::dec; + return false; +} + bool Cia402Protocol::waitVelocityNearZero_(const std::uint8_t node_id, const char* action_name) const { @@ -720,11 +894,11 @@ bool Cia402Protocol::writeAccelerationLimitsToDictionary_( return ok; } -void Cia402Protocol::writeProfilePositionTarget_(const std::uint8_t node_id, - NodeState& state) +bool Cia402Protocol::writeProfilePositionTarget_(const std::uint8_t node_id, + NodeState& state) { if (!bus_runtime_) { - return; + return false; } if (state.profile_velocity <= 0 && state.limit_qd > 0.0) { @@ -736,16 +910,27 @@ void Cia402Protocol::writeProfilePositionTarget_(const std::uint8_t node_id, state.profile_deceleration = profile_acceleration; } - state.controlword = cia402::enableOperationControlword(); - writeNode_(node_id, state); - - state.controlword = cia402::profilePositionControlword(true); - writeControlword_(node_id, state.controlword); - std::this_thread::sleep_for( - std::chrono::milliseconds(config_.profile_position_trigger_delay_ms())); + if (!writeTargetsForMode_(node_id, state.mode, state)) { + return false; + } state.controlword = cia402::profilePositionControlword(false); - writeControlword_(node_id, state.controlword); + if (!writeControlword_(node_id, state.controlword) || + !waitSetPointAcknowledged_(node_id, false)) { + return false; + } + + state.controlword = cia402::profilePositionControlword(true); + if (!writeControlword_(node_id, state.controlword) || + !waitSetPointAcknowledged_(node_id, true)) { + state.controlword = cia402::profilePositionControlword(false); + writeControlword_(node_id, state.controlword); + return false; + } + + state.controlword = cia402::profilePositionControlword(false); + return writeControlword_(node_id, state.controlword) && + waitSetPointAcknowledged_(node_id, false); } bool Cia402Protocol::prepareSafeTargetsForMode_(const std::uint8_t node_id, @@ -782,71 +967,132 @@ bool Cia402Protocol::prepareSafeTargetsForMode_(const std::uint8_t node_id, return true; } -void Cia402Protocol::writeTargetsForMode_(const std::uint8_t node_id, - const msgs::RunMode mode, - const NodeState& state) const +bool Cia402Protocol::writeTargetsForMode_(const std::uint8_t node_id, + const msgs::RunMode mode, + const NodeState& state) const { if (!bus_runtime_) { - return; + return false; } + std::array 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(node_id, msgs::CIA402_TARGET_POSITION_607A, - 0x00, state.target_position); + if (!append(EthercatMotorBusRuntime::makePdoWrite( + 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(node_id, msgs::CIA402_PROFILE_VELOCITY_6081, - 0x00, state.profile_velocity); + if (!append(EthercatMotorBusRuntime::makePdoWrite( + 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(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, - 0x00, state.profile_acceleration); + if (!append(EthercatMotorBusRuntime::makePdoWrite( + 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(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, - 0x00, state.profile_deceleration); + if (!append(EthercatMotorBusRuntime::makePdoWrite( + node_id, msgs::CIA402_PROFILE_DECELERATION_6084, + 0x00, state.profile_deceleration))) { + return false; + } } break; case msgs::RUN_MODE_PROFILE_VELOCITY: - bus_runtime_->writePdo(node_id, msgs::CIA402_TARGET_VELOCITY_60FF, - 0x00, state.target_velocity); + if (!append(EthercatMotorBusRuntime::makePdoWrite( + 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(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, - 0x00, state.profile_acceleration); + if (!append(EthercatMotorBusRuntime::makePdoWrite( + 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(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, - 0x00, state.profile_deceleration); + if (!append(EthercatMotorBusRuntime::makePdoWrite( + node_id, msgs::CIA402_PROFILE_DECELERATION_6084, + 0x00, state.profile_deceleration))) { + return false; + } } break; case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: - bus_runtime_->writePdo(node_id, msgs::CIA402_TARGET_POSITION_607A, - 0x00, state.target_position); - bus_runtime_->writePdo(node_id, msgs::CIA402_TARGET_VELOCITY_60FF, - 0x00, state.target_velocity); + if (!append(EthercatMotorBusRuntime::makePdoWrite( + node_id, msgs::CIA402_TARGET_POSITION_607A, + 0x00, state.target_position)) || + !append(EthercatMotorBusRuntime::makePdoWrite( + node_id, msgs::CIA402_TARGET_VELOCITY_60FF, + 0x00, state.target_velocity))) { + return false; + } break; case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY: - bus_runtime_->writePdo(node_id, msgs::CIA402_TARGET_VELOCITY_60FF, - 0x00, state.target_velocity); + if (!append(EthercatMotorBusRuntime::makePdoWrite( + node_id, msgs::CIA402_TARGET_VELOCITY_60FF, + 0x00, state.target_velocity))) { + return false; + } break; case msgs::RUN_MODE_CYCLIC_SYNC_CURRENT: - bus_runtime_->writePdo(node_id, msgs::CIA402_TARGET_TORQUE_6071, - 0x00, state.target_torque); + if (!append(EthercatMotorBusRuntime::makePdoWrite( + 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(node_id, msgs::CIA402_CONTROL_WORD_6040, 0x00, - state.controlword.value); - bus_runtime_->writePdo(node_id, msgs::CIA402_OPERATION_MODE_6060, 0x00, - toCia402Mode_(state.mode)); - - writeTargetsForMode_(node_id, state.mode, state); + std::array writes{}; + std::size_t count = 0; + writes[count++] = EthercatMotorBusRuntime::makePdoWrite( + node_id, msgs::CIA402_CONTROL_WORD_6040, 0x00, state.controlword.value); + writes[count++] = EthercatMotorBusRuntime::makePdoWrite( + node_id, msgs::CIA402_OPERATION_MODE_6060, 0x00, toCia402Mode_(state.mode)); + if (!appendTargetWritesForMode_(node_id, state.mode, state, + writes.data(), writes.size(), count)) { + return false; + } + return bus_runtime_->writePdosAtomic(writes.data(), count); } } // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_status_monitor.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_status_monitor.cpp new file mode 100644 index 00000000..1e0eeb5f --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_status_monitor.cpp @@ -0,0 +1,284 @@ +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_status_monitor.h" + +#include +#include +#include +#include + +#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 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 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 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 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, 256> nodes{}; + std::size_t node_count = 0; + { + std::lock_guard 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 monitor_lock(monitor_mutex_); + StatusSample current; + readStatusSample_(node_id, expected_operation_enabled, current); + + StatusSample previous; + bool had_previous = false; + { + std::lock_guard 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(node_id); + } + return; + } + if (had_previous && !previous.read_ok) { + CMVR_LOG(INFO) << "[Cia402StatusMonitor] node status snapshot recovered" + << ", node=" << static_cast(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(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(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(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(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( + node_id, msgs::CIA402_STATUS_WORD_6041, 0x00), + EthercatMotorBusRuntime::makePdoRead( + node_id, msgs::CIA402_ERROR_CODE_603F, 0x00), + EthercatMotorBusRuntime::makePdoRead( + node_id, msgs::CIA402_MODE_DISPLAY_6061, 0x00), + EthercatMotorBusRuntime::makePdoRead( + node_id, msgs::CIA402_ACTUAL_POSITION_6064, 0x00), + EthercatMotorBusRuntime::makePdoRead( + node_id, msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00), + EthercatMotorBusRuntime::makePdoRead( + node_id, msgs::CIA402_ACTUAL_TORQUE_6077, 0x00), + }; + if (!bus_runtime_->readPdosAtomic(reads.data(), reads.size())) { + return false; + } + + sample.statusword = EthercatMotorBusRuntime::pdoReadValue(reads[0]); + sample.error_code = EthercatMotorBusRuntime::pdoReadValue(reads[1]); + sample.mode_display = EthercatMotorBusRuntime::pdoReadValue(reads[2]); + sample.actual_position = EthercatMotorBusRuntime::pdoReadValue(reads[3]); + sample.actual_velocity = EthercatMotorBusRuntime::pdoReadValue(reads[4]); + sample.actual_torque = EthercatMotorBusRuntime::pdoReadValue(reads[5]); + sample.read_ok = true; + + const auto status = cia402::statusword(sample.statusword); + sample.operation_enabled = cia402::isOperationEnabled(status); + sample.status_problem = status.fault != 0 || status.warning != 0 || + status.internal_limit_active != 0 || + (expected_operation_enabled && !sample.operation_enabled); + sample.command_blocked = status.fault != 0 || status.warning != 0 || + sample.error_code != 0 || + (expected_operation_enabled && !sample.operation_enabled); + return true; +} + +const char* Cia402StatusMonitor::deviceStateName_(const std::uint16_t statusword) +{ + if ((statusword & 0x004F) == 0x000F) { + return "FaultReactionActive"; + } + if ((statusword & 0x004F) == 0x0008) { + return "Fault"; + } + if ((statusword & 0x006F) == 0x0007) { + return "QuickStopActive"; + } + const auto status = cia402::statusword(statusword); + if (cia402::isOperationEnabled(status)) { + return "OperationEnabled"; + } + if (cia402::hasState(status, cia402::DeviceState::SwitchedOn)) { + return "SwitchedOn"; + } + if (cia402::hasState(status, cia402::DeviceState::ReadyToSwitchOn)) { + return "ReadyToSwitchOn"; + } + if (cia402::isSwitchOnDisabled(status)) { + return "SwitchOnDisabled"; + } + return "NotReadyToSwitchOn"; +} + +const char* Cia402StatusMonitor::errorCodeDescription_(const std::uint16_t error_code) +{ + switch (error_code) { + case 0x0000: + return "no error"; + case 0x2310: + return "continuous over-current"; + case 0x3210: + return "DC bus over-voltage"; + case 0x3220: + return "DC bus under-voltage"; + case 0x4210: + return "device over-temperature"; + case 0x4310: + return "drive over-temperature"; + case 0x8611: + return "position following error"; + default: + return "unknown or vendor-specific error"; + } +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp index 192ee849..d294bdb0 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp @@ -1,8 +1,11 @@ #include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h" #include +#include #include #include +#include +#include #include #include #include @@ -107,8 +110,11 @@ bool EyouMotor::calibrateZeroQ() return false; } + const auto counts_per_joint_revolution = static_cast( + std::llround(encoder_counts_per_rev_ * gear_ratio_)); std::int32_t zeroed_position = 0; - if (!vendor_adapter_->calibrateZero(node_id_, zeroed_position)) { + if (!vendor_adapter_->calibrateZero(node_id_, counts_per_joint_revolution, + zeroed_position)) { CMVR_LOG(ERROR) << "[EyouMotor] zero calibration failed: " << info_.joint_name; return false; } @@ -121,6 +127,102 @@ bool EyouMotor::calibrateZeroQ() return true; } +bool EyouMotor::commandCyclicPositionsAtomic( + const std::vector>& motors, + const std::vector& positions, + const std::vector& velocities) +{ + if (motors.empty() || motors.size() != positions.size() || + motors.size() != velocities.size() || motors.size() > 256) { + return false; + } + + std::array lock_order{}; + std::array commands{}; + std::shared_ptr protocol; + for (std::size_t i = 0; i < motors.size(); ++i) { + const auto motor = std::dynamic_pointer_cast(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{}); + std::array, 256> locks{}; + for (std::size_t i = 0; i < motors.size(); ++i) { + locks[i] = std::unique_lock(lock_order[i]->mtx_); + } + return protocol && protocol->commandCyclicPositionsAtomic(commands.data(), motors.size()); +} + +bool EyouMotor::readFeedbacksAtomic( + const std::vector>& motors, + std::vector& positions, + std::vector& velocities) +{ + if (motors.empty() || motors.size() > 256) { + return false; + } + + std::array lock_order{}; + std::array feedbacks{}; + std::shared_ptr protocol; + for (std::size_t i = 0; i < motors.size(); ++i) { + const auto motor = std::dynamic_pointer_cast(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{}); + std::array, 256> locks{}; + for (std::size_t i = 0; i < motors.size(); ++i) { + locks[i] = std::unique_lock(lock_order[i]->mtx_); + } + if (!protocol || !protocol->readFeedbacksAtomic(feedbacks.data(), motors.size())) { + return false; + } + + positions.resize(motors.size()); + velocities.resize(motors.size()); + for (std::size_t i = 0; i < motors.size(); ++i) { + positions[i] = feedbacks[i].q; + velocities[i] = feedbacks[i].qd; + } + return true; +} + bool EyouMotor::brakeRelease() { std::scoped_lock lock(mtx_); diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp index dd168318..99848f49 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp @@ -9,6 +9,7 @@ #include "cmvr/msgs/cia402.pb.h" #include "common/base/logging/logger.h" +#include "common/math/support_functions.h" #include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h" namespace cmvr::device { @@ -93,96 +94,250 @@ bool EyouMotorAdapter::writeVelocityLimit(const std::uint8_t node_id, } bool EyouMotorAdapter::calibrateZero(const std::uint8_t node_id, + const std::int64_t counts_per_joint_revolution, std::int32_t& zeroed_position) { if (!bus_runtime_) { return false; } + const auto& zero_config = bus_runtime_->config().zero_calibration(); + const auto home_offset_timeout = + std::chrono::milliseconds{zero_config.timeout_ms()}; + const auto home_offset_poll_period = + std::chrono::milliseconds{zero_config.poll_period_ms()}; + const auto home_offset_stable_samples = zero_config.stable_sample_count(); + const auto home_offset_position_tolerance_counts = + zero_config.position_tolerance_counts(); + const auto home_offset_stable_delta_counts = + zero_config.stable_delta_counts(); + + std::uint32_t original_soft_limit_state = 0; + std::int32_t original_home_offset = 0; + std::int32_t original_position = 0; + if (!bus_runtime_->readSdo( + node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003, + 0x00, original_soft_limit_state) || + !bus_runtime_->readSdo( + node_id, msgs::CIA402_HOME_OFFSET_607C, + 0x00, original_home_offset) || + !bus_runtime_->readSdo( + node_id, msgs::CIA402_ACTUAL_POSITION_6064, + 0x00, original_position)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to snapshot calibration state, node=" + << static_cast(node_id); + return false; + } + + // EYOU applies HomeOffset additively, so clearing it exposes this raw position. + const auto expected_cleared_position_wide = + static_cast(original_position) - + static_cast(original_home_offset); + if (expected_cleared_position_wide < std::numeric_limits::min() || + expected_cleared_position_wide > std::numeric_limits::max()) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] cleared position would overflow, node=" + << static_cast(node_id) + << ", original_position=" << original_position + << ", original_home_offset=" << original_home_offset; + return false; + } + const auto expected_cleared_position = + static_cast(expected_cleared_position_wide); + + const auto write_home_offset = [&](const std::int32_t value) { + return bus_runtime_->writeSdo( + node_id, msgs::CIA402_HOME_OFFSET_607C, 0x00, value); + }; + const auto save_parameters = [&]() { + return bus_runtime_->writeSdo( + node_id, eyou::EYOU_STORE_PARAMETERS_1010, + 0x01, 0x65766173); + }; + const auto wait_for_soft_limit = [&](const std::uint32_t expected_state) { + const auto deadline = std::chrono::steady_clock::now() + home_offset_timeout; + do { + std::uint32_t actual_soft_limit_state = 0; + if (bus_runtime_->readSdo( + node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003, + 0x00, actual_soft_limit_state) && + actual_soft_limit_state == expected_state) { + return true; + } + std::this_thread::sleep_for(home_offset_poll_period); + } while (std::chrono::steady_clock::now() < deadline); + return false; + }; + const auto restore_soft_limit = [&]() { + return bus_runtime_->writeSdo( + node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003, + 0x00, original_soft_limit_state) && + wait_for_soft_limit(original_soft_limit_state); + }; + const auto wait_for_position = [&](const char* phase, + const std::int32_t expected_offset, + const std::int32_t expected_position, + std::int32_t& observed_position) { + const auto deadline = std::chrono::steady_clock::now() + home_offset_timeout; + std::uint32_t stable_samples = 0; + bool has_previous_position = false; + std::int32_t previous_position = 0; + std::int32_t observed_offset = 0; + + do { + const bool read_ok = + bus_runtime_->readSdo( + node_id, msgs::CIA402_HOME_OFFSET_607C, + 0x00, observed_offset) && + bus_runtime_->readSdo( + node_id, msgs::CIA402_ACTUAL_POSITION_6064, + 0x00, observed_position); + const bool position_stable = + !has_previous_position || + SupportFunctions::cyclicAbsoluteDifference( + observed_position, previous_position, + counts_per_joint_revolution) <= home_offset_stable_delta_counts; + const bool sample_matches = + read_ok && observed_offset == expected_offset && + SupportFunctions::cyclicAbsoluteDifference( + observed_position, expected_position, + counts_per_joint_revolution) <= home_offset_position_tolerance_counts && + position_stable; + + stable_samples = sample_matches ? stable_samples + 1 : 0; + if (stable_samples >= home_offset_stable_samples) { + return true; + } + + if (read_ok) { + previous_position = observed_position; + has_previous_position = true; + } + std::this_thread::sleep_for(home_offset_poll_period); + } while (std::chrono::steady_clock::now() < deadline); + + CMVR_LOG(ERROR) << "[EyouMotorAdapter] timed out waiting for home offset state, node=" + << static_cast(node_id) + << ", phase=" << phase + << ", expected_offset=" << expected_offset + << ", actual_offset=" << observed_offset + << ", expected_position=" << expected_position + << ", actual_position=" << observed_position + << ", cyclic_position_distance=" + << SupportFunctions::cyclicAbsoluteDifference( + observed_position, expected_position, + counts_per_joint_revolution) + << ", counts_per_joint_revolution=" + << counts_per_joint_revolution + << ", stable_samples=" << stable_samples; + return false; + }; + // Follow EYOU's required clear -> set -> save sequence during rollback too. + const auto rollback = [&](const char* failed_phase) { + const bool clear_written = write_home_offset(0); + std::int32_t cleared_position = 0; + const bool clear_applied = + clear_written && + wait_for_position("rollback_clear_home_offset", 0, + expected_cleared_position, cleared_position); + const bool offset_written = write_home_offset(original_home_offset); + std::int32_t restored_position = 0; + const bool offset_applied = + offset_written && + wait_for_position("rollback_apply_home_offset", original_home_offset, + original_position, restored_position); + const bool parameters_saved = offset_written && save_parameters(); + const bool saved_state_confirmed = + parameters_saved && + wait_for_position("rollback_save_home_offset", original_home_offset, + original_position, restored_position); + const bool soft_limit_restored = restore_soft_limit(); + const bool rollback_ok = clear_applied && offset_applied && + saved_state_confirmed && soft_limit_restored; + CMVR_LOG(ERROR) << "[EyouMotorAdapter] calibration failed and original state was " + << (rollback_ok ? "restored" : "not fully restored") + << ", node=" << static_cast(node_id) + << ", phase=" << failed_phase + << ", original_home_offset=" << original_home_offset + << ", original_soft_limit_state=" << original_soft_limit_state + << ", clear_applied=" << clear_applied + << ", offset_applied=" << offset_applied + << ", saved_state_confirmed=" << saved_state_confirmed + << ", soft_limit_restored=" << soft_limit_restored; + return false; + }; + if (!bus_runtime_->writeSdo(node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003, 0x00, 0)) { + const bool soft_limit_restored = restore_soft_limit(); CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to disable software position " << "limit before home offset calibration, node=" - << static_cast(node_id); + << static_cast(node_id) + << ", soft_limit_restored=" << soft_limit_restored; return false; } + if (!wait_for_soft_limit(0)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] software position limit did not disable, node=" + << static_cast(node_id); + return rollback("disable_soft_limit"); + } - if (!bus_runtime_->writeSdo(node_id, msgs::CIA402_HOME_OFFSET_607C, - 0x00, 0)) { + if (!write_home_offset(0)) { CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to clear home offset, node=" << static_cast(node_id); - return false; + return rollback("clear_home_offset"); } - std::this_thread::sleep_for(std::chrono::milliseconds{50}); std::int32_t actual_position = 0; - if (!bus_runtime_->readSdo(node_id, msgs::CIA402_ACTUAL_POSITION_6064, - 0x00, actual_position)) { - CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read actual position " - << "after clearing home offset, node=" - << static_cast(node_id); - return false; + if (!wait_for_position("clear_home_offset", 0, + expected_cleared_position, actual_position)) { + return rollback("wait_for_cleared_position"); } - if (actual_position == std::numeric_limits::min()) { CMVR_LOG(ERROR) << "[EyouMotorAdapter] invalid actual position for home " << "offset calibration, node=" << static_cast(node_id) << ", actual_position=" << actual_position; - return false; + return rollback("negate_actual_position"); } const auto home_offset = static_cast(-actual_position); - if (!bus_runtime_->writeSdo(node_id, msgs::CIA402_HOME_OFFSET_607C, - 0x00, home_offset)) { + if (!write_home_offset(home_offset)) { CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write home offset, node=" << static_cast(node_id) << ", home_offset=" << home_offset; - return false; + return rollback("write_home_offset"); } - if (!bus_runtime_->writeSdo( - node_id, eyou::EYOU_STORE_PARAMETERS_1010, - 0x01, - 0x65766173)) { + if (!wait_for_position("apply_home_offset", home_offset, 0, + zeroed_position)) { + return rollback("wait_for_zero_before_save"); + } + + if (!save_parameters()) { CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to save home offset parameter, node=" << static_cast(node_id); - return false; + return rollback("save_parameters"); } - std::this_thread::sleep_for(std::chrono::milliseconds{50}); - std::int32_t home_offset_readback = 0; - if (!bus_runtime_->readSdo(node_id, msgs::CIA402_HOME_OFFSET_607C, - 0x00, home_offset_readback)) { - CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read back home offset, node=" - << static_cast(node_id); - return false; + if (!wait_for_position("save_home_offset", home_offset, 0, + zeroed_position)) { + return rollback("wait_for_zero_after_save"); } - if (home_offset_readback != home_offset) { - CMVR_LOG(ERROR) << "[EyouMotorAdapter] home offset readback mismatch, node=" + + if (!restore_soft_limit()) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to restore software position " + << "limit after home offset calibration, node=" << static_cast(node_id) - << ", expected=" << home_offset - << ", actual=" << home_offset_readback; - return false; - } - - if (!bus_runtime_->readSdo(node_id, msgs::CIA402_ACTUAL_POSITION_6064, - 0x00, zeroed_position)) { - CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read actual position " - << "after writing home offset, node=" - << static_cast(node_id); - return false; - } - if (std::abs(static_cast(zeroed_position)) > 10000) { - CMVR_LOG(ERROR) << "[EyouMotorAdapter] home offset did not zero actual position, " - << "node=" << static_cast(node_id) - << ", actual_position_before=" << actual_position - << ", home_offset=" << home_offset - << ", home_offset_readback=" << home_offset_readback - << ", actual_position_after=" << zeroed_position - << ", tolerance_counts=" << 10000; - return false; + << ", original_soft_limit_state=" << original_soft_limit_state; + return rollback("restore_soft_limit"); } + CMVR_LOG(INFO) << "[EyouMotorAdapter] home offset calibration completed, node=" + << static_cast(node_id) + << ", original_home_offset=" << original_home_offset + << ", cleared_position=" << actual_position + << ", home_offset=" << home_offset + << ", zeroed_position=" << zeroed_position + << ", soft_limit_state=" << original_soft_limit_state; return true; } diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp index ae48c41d..3b877f00 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp @@ -7,6 +7,8 @@ #include #include #include +#include +#include #include @@ -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 kFourMotorIds{1, 2, 3, 4,5}; + "devices/motor/ethercat_motors_four_real_test.pb.txt"; +constexpr std::array kFourMotorIds{1, 2, 3, 4}; constexpr std::chrono::milliseconds kCyclicCommandPeriod{1}; -constexpr std::chrono::milliseconds kPrintPeriod{100}; 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 kFourMotorPhaseRad{0.0, 0.0, 0.0, 0.0,0}; +constexpr double kRaisedCosineCoefficientRad = 0.2; +constexpr double kFourMotorPeriodS = 2.0; +constexpr std::array 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 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 motor_manager_; +}; + +class MultiMotorSafetyGuard { +public: + explicit MultiMotorSafetyGuard( + const std::vector>& 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>& 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(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(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(kMotorManagerId); ASSERT_NE(motor_manager, nullptr); + MotorManagerStopGuard motor_manager_stop_guard(motor_manager); - std::array, kFourMotorIds.size()> motors; + std::vector> 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(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 center_q{}; - for (std::size_t i = 0; i < motors.size(); ++i) { - center_q[i] = motors[i]->getQ(); - } + std::vector center_q; + std::vector 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 error_stats; + std::vector target_q(motors.size(), 0.0); + std::vector target_qd(motors.size(), 0.0); + std::vector actual_q; + std::array minimum_actual_q{}; + std::array 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(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(now - start_time).count(); - std::array target_q{}; - std::array 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::max(); double max_error = std::numeric_limits::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,32 +312,25 @@ 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); - } - - 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)); + 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(skipped_periods); + next_command_time += skipped_periods * kCyclicCommandPeriod; } - 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) { printMotorState(kFourMotorIds[i], motors[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 diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp index 92172420..0da334ef 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp @@ -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(kMotorId, msgs::CIA402_STATUS_WORD_6041, 0x00, statusword); @@ -171,8 +175,6 @@ void printRawEthercatFeedback(const char* label, actual_velocity); runtime->readPdo(kMotorId, msgs::CIA402_ACTUAL_TORQUE_6077, 0x00, actual_torque); - runtime->readPdo(kMotorId, msgs::CIA402_ACTUAL_CURRENT_6078, 0x00, - actual_current); runtime->readPdo(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(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}); diff --git a/cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h b/cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h index 5eb7b5e3..57c97387 100644 --- a/cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h +++ b/cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h @@ -21,7 +21,7 @@ public: std::string typeName() const override { return "MujocoMotor"; } bool init() override; - void setMode(msgs::RunMode mode) override; + bool setMode(msgs::RunMode mode) override; msgs::RunMode getMode() override; bool torqueOn() override; bool torqueOff() override; @@ -45,9 +45,10 @@ public: double getQ() override; double getQd() override; - static bool setTargetsAtomic(const std::vector>& motors, - const std::vector& positions, - const std::vector& velocities); + static bool commandCyclicPositionsAtomic( + const std::vector>& motors, + const std::vector& positions, + const std::vector& velocities); private: bool holdPosition_(); diff --git a/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp b/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp index 07b0bda4..447e8d18 100644 --- a/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp +++ b/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp @@ -42,10 +42,11 @@ bool MujocoMotor::init() return true; } -void MujocoMotor::setMode(const msgs::RunMode mode) +bool MujocoMotor::setMode(const msgs::RunMode mode) { std::scoped_lock lock(mtx_); mode_ = mode; + return true; } msgs::RunMode MujocoMotor::getMode() @@ -216,9 +217,10 @@ double MujocoMotor::getQd() return qd; } -bool MujocoMotor::setTargetsAtomic(const std::vector>& motors, - const std::vector& positions, - const std::vector& velocities) +bool MujocoMotor::commandCyclicPositionsAtomic( + const std::vector>& motors, + const std::vector& positions, + const std::vector& velocities) { if (motors.size() != positions.size() || motors.size() != velocities.size() || motors.empty()) { return false; @@ -233,7 +235,7 @@ bool MujocoMotor::setTargetsAtomic(const std::vector(motors[i]); if (!motor) { return false; } @@ -263,9 +265,13 @@ bool MujocoMotor::setTargetsAtomic(const std::vectormtx_); - motors[i]->target_q_ = clamped_positions[i]; - motors[i]->mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION; + const auto motor = std::dynamic_pointer_cast(motors[i]); + if (!motor) { + return false; + } + std::scoped_lock lock(motor->mtx_); + motor->target_q_ = clamped_positions[i]; + motor->mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION; } return true; } diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h index 0588fd3f..3b64a284 100644 --- a/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h @@ -30,7 +30,7 @@ namespace cmvr { bool initNode(uint8_t node_id) override; - void setMode(uint8_t node_id, msgs::RunMode mode) override; + bool setMode(uint8_t node_id, msgs::RunMode mode) override; void setLimitQ(uint8_t node_id, double ub, double lb) override; void setLimitQd(uint8_t node_id, double qd) override; void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) override; diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp b/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp index 69e1f562..14937d69 100644 --- a/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp @@ -263,7 +263,7 @@ void Ti5MotorCanopenProtocol::writeProfilePositionTargetBySdo(uint8_t node_id, i seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); } -void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { +bool Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { // cur_mode_[node_id] = mode; @@ -334,8 +334,16 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { } default: // TODO: Handle unspecified or unknown mode - break; + return false; } + + if (!waitUntil([&]() { return getMode(node_id) == mode; }, 500)) { + CMVR_LOG(ERROR) << "motor " << static_cast(node_id) + << ": operation mode switch failed, target_mode=" + << static_cast(mode); + return false; + } + return true; } void Ti5MotorCanopenProtocol::seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms) { @@ -563,8 +571,7 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) { } bool Ti5MotorCanopenProtocol::torqueOn(uint8_t node_id) { - setMode(node_id, msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - return true; + return setMode(node_id, msgs::RUN_MODE_CYCLIC_SYNC_POSITION); } bool Ti5MotorCanopenProtocol::brakeRelease(uint8_t node_id) { diff --git a/cmvr-es/devices/motor/manager/include/motor_manager.h b/cmvr-es/devices/motor/manager/include/motor_manager.h index 3bbd2cab..3673355a 100644 --- a/cmvr-es/devices/motor/manager/include/motor_manager.h +++ b/cmvr-es/devices/motor/manager/include/motor_manager.h @@ -45,6 +45,14 @@ public: std::shared_ptr getMotor(std::uint8_t node_id) const; std::shared_ptr getMotor(const std::string& joint_name) const; const std::unordered_map>& motorsMap() const; + bool commandCyclicPositionsAtomic( + const std::vector>& motors, + const std::vector& positions, + const std::vector& velocities) const; + bool readFeedbacksAtomic( + const std::vector>& motors, + std::vector& positions, + std::vector& velocities) const; static std::shared_ptr managerFor(const std::string& id); static std::shared_ptr mujocoWorldFor(const std::string& id); diff --git a/cmvr-es/devices/motor/manager/src/motor_manager.cpp b/cmvr-es/devices/motor/manager/src/motor_manager.cpp index 5e83b043..3a6b5118 100644 --- a/cmvr-es/devices/motor/manager/src/motor_manager.cpp +++ b/cmvr-es/devices/motor/manager/src/motor_manager.cpp @@ -239,6 +239,46 @@ const std::unordered_map>& MotorMana return motors_by_joint_; } +bool MotorManager::commandCyclicPositionsAtomic( + const std::vector>& motors, + const std::vector& positions, + const std::vector& velocities) const +{ + if (motors.empty() || motors.size() != positions.size() || + motors.size() != velocities.size()) { + return false; + } + + if (std::dynamic_pointer_cast(motors.front())) { + return EyouMotor::commandCyclicPositionsAtomic(motors, positions, velocities); + } + if (std::dynamic_pointer_cast(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>& motors, + std::vector& positions, + std::vector& velocities) const +{ + if (motors.empty()) { + return false; + } + + if (std::dynamic_pointer_cast(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::managerFor(const std::string& id) { std::lock_guard lock(registry_mutex_); diff --git a/cmvr-es/devices/motor/motor_protocol_interface.h b/cmvr-es/devices/motor/motor_protocol_interface.h index 4b8f3e34..6afbcb64 100644 --- a/cmvr-es/devices/motor/motor_protocol_interface.h +++ b/cmvr-es/devices/motor/motor_protocol_interface.h @@ -27,7 +27,7 @@ namespace cmvr { */ virtual bool initNode(uint8_t node_id) = 0; - virtual void setMode(uint8_t node_id,msgs::RunMode mode ) = 0; + virtual bool setMode(uint8_t node_id,msgs::RunMode mode ) = 0; virtual msgs::RunMode getMode(uint8_t node_id) = 0; virtual void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) = 0; virtual void setLimitQd(uint8_t node_id,double qd) = 0; diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-136-generic/ethercat/devices/ec_generic.ko b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-136-generic/ethercat/devices/ec_generic.ko new file mode 100644 index 00000000..64be6a8d Binary files /dev/null and b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-136-generic/ethercat/devices/ec_generic.ko differ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-136-generic/ethercat/examples/mini/ec_mini.ko b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-136-generic/ethercat/examples/mini/ec_mini.ko new file mode 100644 index 00000000..1e00b08c Binary files /dev/null and b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-136-generic/ethercat/examples/mini/ec_mini.ko differ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-136-generic/ethercat/master/ec_master.ko b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-136-generic/ethercat/master/ec_master.ko new file mode 100644 index 00000000..347595af Binary files /dev/null and b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-136-generic/ethercat/master/ec_master.ko differ diff --git a/docs/ethercat_motor_tutorial.md b/docs/ethercat_motor_tutorial.md index 2ba49119..e1565180 100644 --- a/docs/ethercat_motor_tutorial.md +++ b/docs/ethercat_motor_tutorial.md @@ -75,6 +75,14 @@ motor { stopped_velocity_tolerance_rad_s: 0.001 } + zero_calibration { + timeout_ms: 2000 + poll_period_ms: 10 + stable_sample_count: 5 + position_tolerance_counts: 10000 + stable_delta_counts: 1000 + } + slaves { motor_id: 1 alias: 0 position: 0 } slaves { motor_id: 2 alias: 0 position: 1 } slaves { motor_id: 3 alias: 0 position: 2 } diff --git a/protos/cmvr/config/motor_config/motor_config.proto b/protos/cmvr/config/motor_config/motor_config.proto index c31a4d8a..7c166df9 100644 --- a/protos/cmvr/config/motor_config/motor_config.proto +++ b/protos/cmvr/config/motor_config/motor_config.proto @@ -25,13 +25,20 @@ message EthercatSlaveConfig { } message Cia402ProtocolConfig { - uint32 profile_position_trigger_delay_ms = 1; uint32 state_transition_timeout_ms = 2; uint32 velocity_stop_timeout_ms = 3; uint32 status_poll_period_ms = 4; double stopped_velocity_tolerance_rad_s = 5; } +message ZeroCalibrationConfig { + uint32 timeout_ms = 1; + uint32 poll_period_ms = 2; + uint32 stable_sample_count = 3; + uint32 position_tolerance_counts = 4; + uint32 stable_delta_counts = 5; +} + message EtherCATDcConfig { optional bool enable = 1; optional int32 reference_motor_id = 2; @@ -54,6 +61,7 @@ message EtherCATConfig { EtherCATDcConfig dc = 4; optional uint32 slave_op_timeout_ms = 5; optional uint32 slave_state_poll_period_ms = 6; + ZeroCalibrationConfig zero_calibration = 7; repeated EthercatSlaveConfig slaves = 10; } diff --git a/script/ethercat/start_ethercat.sh b/script/ethercat/start_ethercat.sh index 269aef67..c1b9deeb 100755 --- a/script/ethercat/start_ethercat.sh +++ b/script/ethercat/start_ethercat.sh @@ -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