diff --git a/.github/workflows/ci_conda.yml b/.github/workflows/ci_conda.yml index 539cf2e..a303e94 100644 --- a/.github/workflows/ci_conda.yml +++ b/.github/workflows/ci_conda.yml @@ -51,7 +51,8 @@ jobs: mkdir -p build cd build cmake -GNinja -DCMAKE_INSTALL_PREFIX=$CONDA_PREFIX \ - -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} .. + -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \ + -DBUILD_TESTING=ON .. - name: "🏗️ Build [Linux]" if: contains(matrix.os, 'ubuntu') diff --git a/CMakeLists.txt b/CMakeLists.txt index eb663d0..34941e8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ cmake_minimum_required(VERSION 3.8...3.30) project( YarpCiA402 # project name - VERSION 0.5.0 # bump as you tag releases + VERSION 0.6.0 # bump as you tag releases DESCRIPTION "YARP device plugin for EtherCAT CiA402 drives" LANGUAGES CXX ) @@ -74,6 +74,11 @@ add_subdirectory(src) add_subdirectory(utils) add_subdirectory(device) -# add_subdirectory(test) +# Add tests if BUILD_TESTING is enabled +option(BUILD_TESTING "Build the testing tree." OFF) +if(BUILD_TESTING) + enable_testing() + add_subdirectory(test) +endif() include(AddUninstallTarget) diff --git a/README.md b/README.md index 7964e8b..e59e2ad 100644 --- a/README.md +++ b/README.md @@ -17,7 +17,7 @@ Ensure the following dependencies are installed: 1. Clone the repository: ```bash - git clone https://github.com/your-repo/yarp-device-ethercat-cia402.git + git clone https://github.com/ami-iit/yarp-device-ethercat-cia402.git cd yarp-device-ethercat-cia402 ``` 2. Create a build directory and navigate into it: @@ -66,6 +66,47 @@ To run the plugin with a specific configuration: yarprobotinterface --config config/robot/template_1_motor/config.xml ``` +## Testing 🧪 + +This project includes a hardware-in-the-loop test suite built with [Catch2](https://github.com/catchorg/Catch2). + +### Building Tests + +To build the tests, enable the `BUILD_TESTING` option: + +```bash +cd build +cmake .. -DBUILD_TESTING=ON +cmake --build . +``` + +### Running Tests + +The hardware-in-the-loop test requires actual hardware connected via EtherCAT. For detailed instructions, see [test/HardwareInTheLoopTest/README.md](test/HardwareInTheLoopTest/README.md). + +To run the tests: + +```bash +cd build +# Run all tests labeled as "hardware" +ctest -L hardware -V + +# Or run the test executable directly +./bin/HardwareInTheLoopTest +``` + +**Note**: The test requires network capabilities to access the EtherCAT interface: +```bash +sudo setcap cap_net_raw,cap_net_admin+ep ./bin/HardwareInTheLoopTest +``` + +The test validates: +- Device loading and initialization +- All control interfaces (position, velocity, torque, current) +- Encoder reading (motor and joint) +- Control mode switching +- Setpoint/measurement validation + ## Supported Drives 🛠️ This plugin has been primarily tested with Synapticon drives. While it may be compatible with other EtherCAT drive models or manufacturers, some modifications might be necessary to ensure proper functionality. This is due to the plugin’s use of a custom Process Data Object (PDO) mapping, which extends beyond the standard CiA402 specification. diff --git a/cmake/AddApplication.cmake b/cmake/AddApplication.cmake index d1dd408..7d4eff1 100644 --- a/cmake/AddApplication.cmake +++ b/cmake/AddApplication.cmake @@ -34,7 +34,7 @@ function(add_application) target_compile_definitions(${name} PRIVATE -D_USE_MATH_DEFINES) set_target_properties(${name} PROPERTIES - OUTPUT_NAME "${PROJECT_NAME}-${name}" + OUTPUT_NAME "yarp-cia402-${name}" VERSION ${PROJECT_VERSION}) target_link_libraries(${name} PRIVATE ${link_libraries}) diff --git a/config/robot/template_1_motor/motion_control/all_joint_mc.xml b/config/robot/template_1_motor/motion_control/all_joint_mc.xml index da1efeb..1bd0533 100644 --- a/config/robot/template_1_motor/motion_control/all_joint_mc.xml +++ b/config/robot/template_1_motor/motion_control/all_joint_mc.xml @@ -5,6 +5,7 @@ BSD-3-Clause license. --> "eth0" 1 + 4e-4 5000 false ( "joint1" ) @@ -17,4 +18,7 @@ BSD-3-Clause license. --> ( "606C" ) ( 0.1 ) ( 100 ) + ( -23.0 ) + ( 23.0 ) + ( false ) diff --git a/device/CiA402MotionControl/CiA402MotionControl.cpp b/device/CiA402MotionControl/CiA402MotionControl.cpp index e444915..7b9d50f 100644 --- a/device/CiA402MotionControl/CiA402MotionControl.cpp +++ b/device/CiA402MotionControl/CiA402MotionControl.cpp @@ -925,7 +925,29 @@ struct CiA402MotionControl::Impl rx->Controlword |= (1u << 5); // Change set immediately rx->Controlword &= ~(1u << 4); // make sure New set-point is low first posLatched[j] = true; - // no command until user calls positionMove/relativeMove + + // compute seed from current measured joint angle + const double currentJointDeg = this->variables.jointPositions[j]; + const int32_t seedStoreCounts + = this->jointDegToTargetCounts(j, currentJointDeg); + + // Apply motion-sense inversion: counts stored in the drive follow the loop + // shaft. + const int32_t seedDriveCounts = this->invertedMotionSenseDirection[j] + ? -seedStoreCounts + : seedStoreCounts; + + // sync drive position (0x6064) to current position + rx->TargetPosition = seedDriveCounts; + + setPoints.targetCounts[j] = seedStoreCounts; + setPoints.jointPositionsDeg[j] = currentJointDeg; + setPoints.isRelative[j] = false; + + if (!setPoints.hasPosSP[j] && !setPoints.pulseHi[j]) + { + setPoints.pulseHi[j] = true; // sync drive target to current position + } } else { // (A) Always assert bit 5 in PP @@ -939,6 +961,7 @@ struct CiA402MotionControl::Impl } // (C) New set-point pending? write 0x607A and raise bit4 + int32_t targetPositionCounts = 0; if (setPoints.hasPosSP[j] || setPoints.pulseHi[j]) { // Absolute/Relative selection (CW bit 6) @@ -948,9 +971,9 @@ struct CiA402MotionControl::Impl rx->Controlword &= ~(1u << 6); // Target position (0x607A) - rx->TargetPosition = this->invertedMotionSenseDirection[j] - ? -setPoints.targetCounts[j] - : setPoints.targetCounts[j]; + targetPositionCounts = this->invertedMotionSenseDirection[j] + ? -setPoints.targetCounts[j] + : setPoints.targetCounts[j]; // New set-point pulse (rising edge) rx->Controlword |= (1u << 4); @@ -959,7 +982,27 @@ struct CiA402MotionControl::Impl setPoints.hasPosSP[j] = false; setPoints.pulseHi[j] = false; setPoints.pulseCoolDown[j] = true; + } else + { + auto tx = this->ethercatManager.getTxView(s); + targetPositionCounts = this->invertedMotionSenseDirection[j] + ? -setPoints.targetCounts[j] + : setPoints.targetCounts[j]; + if (tx.has(CiA402::TxField::Position6064)) + { + targetPositionCounts = tx.get(CiA402::TxField::Position6064, + targetPositionCounts); + } + + setPoints.targetCounts[j] = this->invertedMotionSenseDirection[j] + ? -targetPositionCounts + : targetPositionCounts; + setPoints.jointPositionsDeg[j] + = this->targetCountsToJointDeg(j, targetPositionCounts); + setPoints.isRelative[j] = false; } + + rx->TargetPosition = targetPositionCounts; } } @@ -1829,6 +1872,22 @@ bool CiA402MotionControl::open(yarp::os::Searchable& cfg) m_impl->invertedMotionSenseDirection)) return false; + auto vecBoolToString = [](const std::vector& v) -> std::string { + std::string s; + for (size_t i = 0; i < v.size(); ++i) + { + s += (v[i] ? "true" : "false"); + if (i + 1 < v.size()) + s += ", "; + } + return s; + }; + + yCDebug(CIA402, + "%s: inverted_motion_sense_direction = [%s]", + logPrefix, + vecBoolToString(m_impl->invertedMotionSenseDirection).c_str()); + // --------------------------------------------------------------------- // Initialize the EtherCAT manager (ring in SAFE‑OP) // --------------------------------------------------------------------- @@ -2403,7 +2462,8 @@ void CiA402MotionControl::run() || newActive == VOCAB_CM_FORCE_IDLE) { m_impl->setPoints.reset(j); - m_impl->velLatched[j] = m_impl->trqLatched[j] = m_impl->currLatched[j] = false; + m_impl->velLatched[j] = m_impl->trqLatched[j] = m_impl->posLatched[j] + = m_impl->currLatched[j] = false; if (hwInhibit) { m_impl->controlModeState.target[j] = VOCAB_CM_IDLE; @@ -3403,6 +3463,20 @@ bool CiA402MotionControl::setRefAcceleration(int j, double accDegS2) accDegS2 = -accDegS2; } + // saturate to maximum allowed 10 deg/s^2 + constexpr double maxAcc = 10.0; + if (accDegS2 > maxAcc) + { + yCWarning(CIA402, + "%s: setRefAcceleration: joint %d: acceleration %.2f deg/s^2 too high, " + "saturating to %.2f deg/s^2", + Impl::kClassName.data(), + j, + accDegS2, + maxAcc); + accDegS2 = maxAcc; + } + // Only touch SDOs if PP is ACTIVE int controlMode = -1; { diff --git a/src/CiA402/include/CiA402/EthercatManager.h b/src/CiA402/include/CiA402/EthercatManager.h index 3eaa5f1..df8388c 100644 --- a/src/CiA402/include/CiA402/EthercatManager.h +++ b/src/CiA402/include/CiA402/EthercatManager.h @@ -355,6 +355,7 @@ class EthercatManager int m_lastWkc{0}; ///< Last working counter. int m_expectedWkc{0}; ///< Expected working counter. + int m_consecutivePdoErrors{0}; ///< Number of consecutive PDO failures. char m_ioMap[4096]{}; ///< SOEM IO map buffer (shared). int m_pdoTimeoutUs{EC_TIMEOUTRET}; ///< Receive timeout for process data. diff --git a/src/CiA402/src/EthercatManager.cpp b/src/CiA402/src/EthercatManager.cpp index 3d7bb01..ff4caf5 100644 --- a/src/CiA402/src/EthercatManager.cpp +++ b/src/CiA402/src/EthercatManager.cpp @@ -5,7 +5,9 @@ #include // std +#include #include +#include #include // yarp #include @@ -381,24 +383,55 @@ EthercatManager::Error EthercatManager::sendReceive() noexcept { return Error::NotInitialized; } + constexpr int kImmediateRetries = 3; + // Perform cyclic process data exchange with thread safety. In SAFE-OP, inputs // can still be exchanged but outputs are not applied; allow a best-effort // exchange so that configuration code can probe PDOs. std::lock_guard lk(m_ioMtx); - ecx_send_processdata(&m_ctx); - m_lastWkc = ecx_receive_processdata(&m_ctx, m_pdoTimeoutUs); - if (!m_isOperational) + + auto sendReceiveOnce = [&]() { + ecx_send_processdata(&m_ctx); + m_lastWkc = ecx_receive_processdata(&m_ctx, m_pdoTimeoutUs); + }; + + for (int attempt = 0; attempt < kImmediateRetries; ++attempt) + { + sendReceiveOnce(); + + if (!m_isOperational) + { + // In SAFE-OP there is no strict WKC expectation; treat any receive as success + return (m_lastWkc >= 0) ? Error::NoError : Error::PdoExchangeFailed; + } + + if (m_lastWkc >= m_expectedWkc) + { + m_consecutivePdoErrors = 0; + return Error::NoError; + } + } + + // Too many consecutive failures → attempt state-based recovery. + if (m_consecutivePdoErrors < std::numeric_limits::max()) { - // In SAFE-OP there is no strict WKC expectation; treat any receive as success - return (m_lastWkc >= 0) ? Error::NoError : Error::PdoExchangeFailed; + ++m_consecutivePdoErrors; } - if (m_lastWkc >= m_expectedWkc) - return Error::NoError; - // quick retry once: re-read state and try a short receive - ecx_readstate(&m_ctx); - m_lastWkc = ecx_receive_processdata(&m_ctx, m_pdoTimeoutUs); - return (m_lastWkc >= m_expectedWkc) ? Error::NoError : Error::PdoExchangeFailed; + // Escalate by requesting the monitor thread to intervene after repeated failures. + constexpr int kRecoveryThreshold = 2; + if (m_consecutivePdoErrors >= kRecoveryThreshold) + { + m_ctx.grouplist[0].docheckstate = 1; + yCWarning(CIA402, + "%s: sendReceive: WKC=%d expected=%d (consecutive=%d) → scheduling recovery", + m_kClassName.data(), + m_lastWkc, + m_expectedWkc, + m_consecutivePdoErrors); + } + + return Error::PdoExchangeFailed; } EthercatManager::Error EthercatManager::goOperational() noexcept @@ -438,6 +471,7 @@ EthercatManager::Error EthercatManager::goOperational() noexcept m_runWatch = true; m_watchThread = std::thread(&EthercatManager::errorMonitorLoop, this); m_isOperational = true; + m_consecutivePdoErrors = 0; yCInfo(CIA402,"%s: EtherCAT: ring is OPERATIONAL", m_kClassName.data()); return Error::NoError; } @@ -479,6 +513,7 @@ EthercatManager::Error EthercatManager::goPreOp() noexcept } m_isOperational = false; + m_consecutivePdoErrors = 0; yCInfo(CIA402,"%s: EtherCAT: ring is PRE-OP", m_kClassName.data()); return Error::NoError; } @@ -530,21 +565,160 @@ TxView EthercatManager::getTxView(int slaveIdx) const noexcept void EthercatManager::errorMonitorLoop() noexcept { using namespace std::chrono_literals; + constexpr auto baseSleep = 50ms; + constexpr auto maxSleep = 500ms; + auto sleepDuration = baseSleep; + while (m_runWatch.load()) { + bool shouldCheck = false; + { + std::lock_guard lk(m_ioMtx); + shouldCheck = m_isOperational + && ((m_lastWkc < m_expectedWkc) + || m_ctx.grouplist[0].docheckstate != 0); + } + + if (!shouldCheck) + { + std::this_thread::sleep_for(baseSleep); + continue; + } + + bool actionsPerformed = false; + bool recoveredAny = false; + { std::lock_guard lk(m_ioMtx); + ecx_readstate(&m_ctx); + for (int s = 1; s <= m_ctx.slavecount; ++s) { - if (m_ctx.slavelist[s].state != EC_STATE_OPERATIONAL) + ec_slavet& sl = m_ctx.slavelist[s]; + + if (sl.state == EC_STATE_OPERATIONAL) { - m_ctx.slavelist[s].state = EC_STATE_OPERATIONAL; + sl.islost = 0; + continue; + } + + actionsPerformed = true; + + yCWarning(CIA402, + "%s: monitor: slave %d '%s' state=0x%02X AL=0x%04X islost=%d", + m_kClassName.data(), + s, + sl.name, + sl.state, + sl.ALstatuscode, + sl.islost); + + if ((sl.state & EC_STATE_ERROR) != 0) + { + sl.state = EC_STATE_SAFE_OP + EC_STATE_ACK; + ecx_writestate(&m_ctx, s); + ecx_statecheck(&m_ctx, s, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + } + + if (sl.state == EC_STATE_SAFE_OP) + { + sl.state = EC_STATE_OPERATIONAL; ecx_writestate(&m_ctx, s); + ecx_statecheck(&m_ctx, s, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); + } + + if (sl.state != EC_STATE_OPERATIONAL) + { + if (sl.state == EC_STATE_NONE) + { + sl.islost = 1; + } + + int rc = 0; + if (sl.islost) + { + yCWarning(CIA402, + "%s: monitor: attempting recover on slave %d '%s'", + m_kClassName.data(), + s, + sl.name); + rc = ecx_recover_slave(&m_ctx, static_cast(s), EC_TIMEOUTRET3); + } + else + { + yCWarning(CIA402, + "%s: monitor: attempting reconfig on slave %d '%s'", + m_kClassName.data(), + s, + sl.name); + rc = ecx_reconfig_slave(&m_ctx, static_cast(s), EC_TIMEOUTRET3); + } + + if (rc > 0) + { + sl.islost = 0; + sl.state = EC_STATE_OPERATIONAL; + ecx_writestate(&m_ctx, s); + ecx_statecheck(&m_ctx, s, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); + } + else + { + sl.islost = 1; + } + } + + if (sl.state == EC_STATE_OPERATIONAL) + { + recoveredAny = true; + yCInfo(CIA402, + "%s: monitor: slave %d '%s' restored to OP", + m_kClassName.data(), + s, + sl.name); + } + else + { + yCError(CIA402, + "%s: monitor: slave %d '%s' still not OP (state=0x%02X AL=0x%04X)", + m_kClassName.data(), + s, + sl.name, + sl.state, + sl.ALstatuscode); } } + + if (actionsPerformed) + { + ecx_send_processdata(&m_ctx); + m_lastWkc = ecx_receive_processdata(&m_ctx, m_pdoTimeoutUs); + if (m_lastWkc >= m_expectedWkc) + { + recoveredAny = true; + } + } + + if (m_ctx.grouplist[0].docheckstate) + { + m_ctx.grouplist[0].docheckstate = 0; + } } - std::this_thread::sleep_for(10ms); + + if (recoveredAny) + { + sleepDuration = baseSleep; + } + else if (actionsPerformed) + { + sleepDuration = std::min(sleepDuration * 2, maxSleep); + } + else + { + sleepDuration = baseSleep; + } + + std::this_thread::sleep_for(sleepDuration); } } diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..6e94fed --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,4 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + +add_subdirectory(HardwareInTheLoopTest) diff --git a/test/HardwareInTheLoopTest/CMakeLists.txt b/test/HardwareInTheLoopTest/CMakeLists.txt new file mode 100644 index 0000000..0cedf42 --- /dev/null +++ b/test/HardwareInTheLoopTest/CMakeLists.txt @@ -0,0 +1,59 @@ +# SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + +# Find Catch2 +find_package(Catch2 3 QUIET) + +if(NOT Catch2_FOUND) + message(STATUS "Catch2 not found, trying to fetch it") + include(FetchContent) + FetchContent_Declare( + Catch2 + GIT_REPOSITORY https://github.com/catchorg/Catch2.git + GIT_TAG v3.5.2 + ) + FetchContent_MakeAvailable(Catch2) +endif() + +# Find YARP robotinterface +find_package(YARP_robotinterface REQUIRED) + +# Get YARP data install directory +get_property(YARP_DATA_INSTALL_DIR TARGET YARP::YARP_os PROPERTY INTERFACE_INCLUDE_DIRECTORIES) +list(GET YARP_DATA_INSTALL_DIR 0 YARP_DATA_INSTALL_DIR_FULL) +get_filename_component(YARP_DATA_INSTALL_DIR_FULL "${YARP_DATA_INSTALL_DIR_FULL}" DIRECTORY) +get_filename_component(YARP_DATA_INSTALL_DIR_FULL "${YARP_DATA_INSTALL_DIR_FULL}" DIRECTORY) + +# Create the test executable +add_executable(HardwareInTheLoopTest HardwareInTheLoopTest.cpp) + +target_link_libraries(HardwareInTheLoopTest + PRIVATE + Catch2::Catch2WithMain + YARP::YARP_os + YARP::YARP_dev + YARP::YARP_robotinterface +) + +target_compile_definitions(HardwareInTheLoopTest + PRIVATE + CMAKE_CURRENT_SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}" + CMAKE_BINARY_DIR="${CMAKE_BINARY_DIR}" + YARP_DATA_INSTALL_DIR_FULL="${YARP_DATA_INSTALL_DIR_FULL}" +) + +# Note: This test requires hardware to be connected +# It should be run manually, not in CI +add_test(NAME HardwareInTheLoopTest + COMMAND HardwareInTheLoopTest + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) + +# Set test properties +set_tests_properties(HardwareInTheLoopTest PROPERTIES + LABELS "hardware;manual" + TIMEOUT 60 +) + +# Install the test configuration file +install(FILES test_config.xml + DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME}/tests/HardwareInTheLoopTest) diff --git a/test/HardwareInTheLoopTest/HardwareInTheLoopTest.cpp b/test/HardwareInTheLoopTest/HardwareInTheLoopTest.cpp new file mode 100644 index 0000000..4661eb5 --- /dev/null +++ b/test/HardwareInTheLoopTest/HardwareInTheLoopTest.cpp @@ -0,0 +1,354 @@ +/** + * @copyright 2025 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include +#include + +// Catch2 +#include +#include + +// YARP +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// This function sets the YARP_DATA_DIRS environment variable +bool setYarpDataDirs(const std::string& installPrefix) +{ +#ifdef _WIN32 + if (0 != _putenv_s("YARP_DATA_DIRS", installPrefix.c_str())) + { + return false; + } +#else + if (0 != ::setenv("YARP_DATA_DIRS", installPrefix.c_str(), true)) + { + return false; + } +#endif + return true; +} + +bool ensureYARPDevicesCanBeFound() +{ + // Make sure that the CiA402MotionControl device can be found + std::string yarpDataDirs = YARP_DATA_INSTALL_DIR_FULL; + + // Add the build directory to find the device + yarpDataDirs = yarpDataDirs + ":" + CMAKE_BINARY_DIR + "/share/yarp"; + + return setYarpDataDirs(yarpDataDirs); +} + +TEST_CASE("Hardware-in-the-loop test for CiA402MotionControl") +{ + // Initialize YARP network in local mode (no yarpserver needed) + yarp::os::Network network; + yarp::os::NetworkBase::setLocalMode(true); + + // Ensure YARP_DATA_DIRS contains the path to find the device + REQUIRE(ensureYARPDevicesCanBeFound()); + + // Get the path to the test configuration file + std::filesystem::path configPath = std::filesystem::path(CMAKE_CURRENT_SOURCE_DIR) + / std::filesystem::path("test_config.xml"); + + INFO("Loading yarprobotinterface file from " << configPath.string()); + + // Load the robot interface configuration + yarp::os::Property robotInterfaceConfig; + yarp::robotinterface::XMLReader reader; + yarp::robotinterface::XMLReaderResult result = reader.getRobotFromFile(configPath.string(), + robotInterfaceConfig); + + REQUIRE(result.parsingIsSuccessful); + + // Start the device + INFO("Starting robot interface..."); + REQUIRE(result.robot.enterPhase(yarp::robotinterface::ActionPhaseStartup)); + + // Get the device + REQUIRE(result.robot.hasDevice("test_joint_mc")); + yarp::dev::PolyDriver* driver = result.robot.device("test_joint_mc").driver(); + REQUIRE(driver != nullptr); + + // Get the interfaces + yarp::dev::IEncodersTimed* iEncoders = nullptr; + yarp::dev::IMotorEncoders* iMotorEncoders = nullptr; + yarp::dev::IPositionControl* iPositionControl = nullptr; + yarp::dev::IVelocityControl* iVelocityControl = nullptr; + yarp::dev::ITorqueControl* iTorqueControl = nullptr; + yarp::dev::ICurrentControl* iCurrentControl = nullptr; + yarp::dev::IControlMode* iControlMode = nullptr; + yarp::dev::IAxisInfo* iAxisInfo = nullptr; + yarp::dev::IControlLimits* iControlLimits = nullptr; + + REQUIRE(driver->view(iEncoders)); + REQUIRE(driver->view(iMotorEncoders)); + REQUIRE(driver->view(iPositionControl)); + REQUIRE(driver->view(iVelocityControl)); + REQUIRE(driver->view(iTorqueControl)); + REQUIRE(driver->view(iCurrentControl)); + REQUIRE(driver->view(iControlMode)); + REQUIRE(driver->view(iAxisInfo)); + REQUIRE(driver->view(iControlLimits)); + + REQUIRE(iEncoders != nullptr); + REQUIRE(iMotorEncoders != nullptr); + REQUIRE(iPositionControl != nullptr); + REQUIRE(iVelocityControl != nullptr); + REQUIRE(iTorqueControl != nullptr); + REQUIRE(iCurrentControl != nullptr); + REQUIRE(iControlMode != nullptr); + REQUIRE(iAxisInfo != nullptr); + REQUIRE(iControlLimits != nullptr); + + // Get the number of axes + int numAxes = 0; + REQUIRE(iEncoders->getAxes(&numAxes)); + REQUIRE(numAxes == 1); + + const int axisIndex = 0; + + SECTION("Axis info test") + { + std::string axisName; + REQUIRE(iAxisInfo->getAxisName(axisIndex, axisName)); + CHECK(axisName == "joint1"); + + yarp::dev::JointTypeEnum jointType; + REQUIRE(iAxisInfo->getJointType(axisIndex, jointType)); + } + + SECTION("Control limits test") + { + double min = 0.0, max = 0.0; + REQUIRE(iControlLimits->getLimits(axisIndex, &min, &max)); + INFO("Joint limits: [" << min << ", " << max << "]"); + CHECK(min < max); + } + + SECTION("Encoder reading test") + { + // Wait a bit for the device to settle + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + // Read joint encoder + double jointPosition = 0.0; + REQUIRE(iEncoders->getEncoder(axisIndex, &jointPosition)); + INFO("Joint position: " << jointPosition << " deg"); + + // Read motor encoder + double motorPosition = 0.0; + REQUIRE(iMotorEncoders->getMotorEncoder(axisIndex, &motorPosition)); + INFO("Motor position: " << motorPosition << " deg"); + + // Read encoder with timestamp + double encoderTimestamp = 0.0; + REQUIRE(iEncoders->getEncoderTimed(axisIndex, &jointPosition, &encoderTimestamp)); + CHECK(encoderTimestamp > 0.0); + + // Read motor encoder speed + double motorSpeed = 0.0; + REQUIRE(iMotorEncoders->getMotorEncoderSpeed(axisIndex, &motorSpeed)); + INFO("Motor speed: " << motorSpeed << " deg/s"); + + // Read joint encoder speed + double jointSpeed = 0.0; + REQUIRE(iEncoders->getEncoderSpeed(axisIndex, &jointSpeed)); + INFO("Joint speed: " << jointSpeed << " deg/s"); + } + + SECTION("Control mode switching test") + { + // Test switching to position control mode + REQUIRE(iControlMode->setControlMode(axisIndex, VOCAB_CM_POSITION)); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + int mode = 0; + REQUIRE(iControlMode->getControlMode(axisIndex, &mode)); + CHECK(mode == VOCAB_CM_POSITION); + + // Test switching to velocity control mode + REQUIRE(iControlMode->setControlMode(axisIndex, VOCAB_CM_VELOCITY)); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + REQUIRE(iControlMode->getControlMode(axisIndex, &mode)); + CHECK(mode == VOCAB_CM_VELOCITY); + + // Test switching to torque control mode + REQUIRE(iControlMode->setControlMode(axisIndex, VOCAB_CM_TORQUE)); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + REQUIRE(iControlMode->getControlMode(axisIndex, &mode)); + CHECK(mode == VOCAB_CM_TORQUE); + + // Test switching to current control mode + REQUIRE(iControlMode->setControlMode(axisIndex, VOCAB_CM_CURRENT)); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + REQUIRE(iControlMode->getControlMode(axisIndex, &mode)); + CHECK(mode == VOCAB_CM_CURRENT); + } + + SECTION("Position control test") + { + // Switch to position control mode + REQUIRE(iControlMode->setControlMode(axisIndex, VOCAB_CM_POSITION)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + // Read initial position + double initialPosition = 0.0; + REQUIRE(iEncoders->getEncoder(axisIndex, &initialPosition)); + INFO("Initial position: " << initialPosition << " deg"); + + // Send a small position command (relative to current position) + double targetPosition = initialPosition + 5.0; // Move 5 degrees + REQUIRE(iPositionControl->positionMove(axisIndex, targetPosition)); + + // Wait for motion to start + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + // Read the target position from the device + double readTargetPosition = 0.0; + REQUIRE(iPositionControl->getTargetPosition(axisIndex, &readTargetPosition)); + CHECK_THAT(readTargetPosition, Catch::Matchers::WithinAbs(targetPosition, 0.01)); + + // Check if motion is done + bool motionDone = false; + REQUIRE(iPositionControl->checkMotionDone(axisIndex, &motionDone)); + + // Wait for motion to complete (with timeout) + int waitCount = 0; + while (!motionDone && waitCount < 100) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + REQUIRE(iPositionControl->checkMotionDone(axisIndex, &motionDone)); + waitCount++; + } + + // Read final position + double finalPosition = 0.0; + REQUIRE(iEncoders->getEncoder(axisIndex, &finalPosition)); + INFO("Final position: " << finalPosition << " deg"); + + // Check that we moved in the right direction + if (targetPosition > initialPosition) + { + CHECK(finalPosition > initialPosition); + } + else + { + CHECK(finalPosition < initialPosition); + } + + // Check that we're close to the target (within a reasonable tolerance) + // The tolerance is larger here as we're testing with actual hardware + CHECK_THAT(finalPosition, Catch::Matchers::WithinAbs(targetPosition, 1.0)); + } + + SECTION("Velocity control test") + { + // Switch to velocity control mode + REQUIRE(iControlMode->setControlMode(axisIndex, VOCAB_CM_VELOCITY)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + // Read initial position + double initialPosition = 0.0; + REQUIRE(iEncoders->getEncoder(axisIndex, &initialPosition)); + + // Send a velocity command + double velocitySetpoint = 10.0; // 10 deg/s + REQUIRE(iVelocityControl->velocityMove(axisIndex, velocitySetpoint)); + + // Wait for motion + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + // Read final position + double finalPosition = 0.0; + REQUIRE(iEncoders->getEncoder(axisIndex, &finalPosition)); + + // Check that the joint moved + CHECK(std::abs(finalPosition - initialPosition) > 0.1); + + // Stop the motion + REQUIRE(iVelocityControl->velocityMove(axisIndex, 0.0)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + SECTION("Torque control test") + { + // Switch to torque control mode + REQUIRE(iControlMode->setControlMode(axisIndex, VOCAB_CM_TORQUE)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + // Read torque + double torque = 0.0; + REQUIRE(iTorqueControl->getTorque(axisIndex, &torque)); + INFO("Current torque: " << torque << " Nm"); + + // Send a small torque command + double torqueSetpoint = 0.1; // Small torque + REQUIRE(iTorqueControl->setRefTorque(axisIndex, torqueSetpoint)); + + // Wait a bit + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + // Read torque again + REQUIRE(iTorqueControl->getTorque(axisIndex, &torque)); + INFO("Torque after setpoint: " << torque << " Nm"); + + // Stop the torque + REQUIRE(iTorqueControl->setRefTorque(axisIndex, 0.0)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + SECTION("Current control test") + { + // Switch to current control mode + REQUIRE(iControlMode->setControlMode(axisIndex, VOCAB_CM_CURRENT)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + // Read current + double current = 0.0; + REQUIRE(iCurrentControl->getCurrent(axisIndex, ¤t)); + INFO("Current: " << current << " A"); + + // Send a small current command + double currentSetpoint = 0.1; // Small current + REQUIRE(iCurrentControl->setRefCurrent(axisIndex, currentSetpoint)); + + // Wait a bit + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + // Read current again + REQUIRE(iCurrentControl->getCurrent(axisIndex, ¤t)); + INFO("Current after setpoint: " << current << " A"); + + // Stop the current + REQUIRE(iCurrentControl->setRefCurrent(axisIndex, 0.0)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + // Cleanup + INFO("Halting robot interface..."); + REQUIRE(result.robot.enterPhase(yarp::robotinterface::ActionPhaseInterrupt1)); + REQUIRE(result.robot.enterPhase(yarp::robotinterface::ActionPhaseShutdown)); + + INFO("Test completed successfully."); +} diff --git a/test/HardwareInTheLoopTest/README.md b/test/HardwareInTheLoopTest/README.md new file mode 100644 index 0000000..9b53c13 --- /dev/null +++ b/test/HardwareInTheLoopTest/README.md @@ -0,0 +1,122 @@ +# Hardware-in-the-Loop Test for CiA402MotionControl + +This test validates the CiA402MotionControl device with actual hardware connected via EtherCAT. + +## Prerequisites + +1. **Hardware**: A single-joint EtherCAT ring with a CiA402-compliant drive must be connected to the test machine. +2. **Network Interface**: The EtherCAT network interface must be available (default: `eth0`). +3. **Permissions**: The test executable needs network capabilities to access the EtherCAT interface. +4. **YARP Server**: No YARP server is needed (the test uses YARP in local mode). + +## Building the Test + +To enable and build the test: + +```bash +cd build +cmake .. -DBUILD_TESTING=ON +cmake --build . +``` + +## Running the Test + +### 1. Set up network interface permissions + +The test needs to access the EtherCAT network interface. You can either: + +**Option A**: Run as root (not recommended) +```bash +sudo ./test/HardwareInTheLoopTest/HardwareInTheLoopTest +``` + +**Option B**: Set capabilities on the test executable (recommended) +```bash +sudo setcap cap_net_raw,cap_net_admin+ep ./test/HardwareInTheLoopTest/HardwareInTheLoopTest +./test/HardwareInTheLoopTest/HardwareInTheLoopTest +``` + +### 2. Configure the network interface + +Edit `test/HardwareInTheLoopTest/test_config.xml` to set the correct network interface name: + +```xml +"eth0" +``` + +### 3. Run the test + +```bash +cd build +./test/HardwareInTheLoopTest/HardwareInTheLoopTest +``` + +Or using CTest: +```bash +cd build +ctest -L hardware -V +``` + +## Test Coverage + +The test validates the following functionality: + +1. **Device Loading**: Loads the CiA402MotionControl device via yarprobotinterface +2. **Interface Availability**: Verifies all control interfaces are available +3. **Axis Information**: Reads axis names and joint types +4. **Control Limits**: Reads and validates joint limits +5. **Encoder Reading**: Tests joint and motor encoder reading with timestamps +6. **Control Mode Switching**: Tests switching between different control modes: + - Position control (VOCAB_CM_POSITION) + - Velocity control (VOCAB_CM_VELOCITY) + - Torque control (VOCAB_CM_TORQUE) + - Current control (VOCAB_CM_CURRENT) +7. **Position Control**: Sends position setpoints and validates motion +8. **Velocity Control**: Sends velocity commands and validates motion +9. **Torque Control**: Sends torque setpoints and reads torque feedback +10. **Current Control**: Sends current setpoints and reads current feedback + +## Test Sections + +The test uses Catch2 sections to organize different test cases: +- `Axis info test` +- `Control limits test` +- `Encoder reading test` +- `Control mode switching test` +- `Position control test` +- `Velocity control test` +- `Torque control test` +- `Current control test` + +You can run specific sections using Catch2's command-line options: +```bash +./test/HardwareInTheLoopTest/HardwareInTheLoopTest "[Position control test]" +``` + +## Troubleshooting + +### "Cannot initialize on eth0" +- Check that the network interface exists: `ip link show` +- Verify the interface name in `test_config.xml` +- Check that no other process is using the EtherCAT interface + +### "No slaves found" +- Verify that the EtherCAT hardware is powered on +- Check cable connections +- Verify that the drive is in a ready state + +### "Timeout waiting for motion" +- The drive may have limits or safety features preventing motion +- Check drive configuration and error logs +- Verify that the commanded positions are within limits + +### Permission denied +- Run with sudo or set capabilities: `sudo setcap cap_net_raw,cap_net_admin+ep ` + +## Notes + +- This test is designed for **manual execution** with hardware connected +- The test should **not** be run in CI/CD pipelines without hardware +- The test uses YARP in local mode, so no yarpserver is needed +- Test timeouts are set to 60 seconds to allow for hardware initialization +- The test is marked with labels "hardware" and "manual" for filtering diff --git a/test/HardwareInTheLoopTest/test_config.xml b/test/HardwareInTheLoopTest/test_config.xml new file mode 100644 index 0000000..eddb4d6 --- /dev/null +++ b/test/HardwareInTheLoopTest/test_config.xml @@ -0,0 +1,29 @@ + + + + + + + + "eth0" + 1 + 4e-4 + 5000 + false + ( "joint1" ) + ( false ) + ( "motor" ) + ( "joint" ) + ( "6064" ) + ( "606C" ) + ( "6064" ) + ( "606C" ) + ( 0.1 ) + ( 100 ) + ( -23.0 ) + ( 23.0 ) + ( false ) + +