Add hardware-in-the-loop test suite with Catch2 for CiA402MotionControl device

.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')

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)

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.
 

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

config/robot/template_1_motor/motion_control/all_joint_mc.xml

@@ -5,6 +5,7 @@ BSD-3-Clause license. -->
 <device xmlns:xi="http://www.w3.org/2001/XInclude" name="all_joints_mc" type="CiA402MotionControl">
     <param name="ifname">"eth0"</param>
     <param name="num_axes">1</param>
+    <param name="period">4e-4</param>
     <param name="pdo_timeout_us">5000</param>
     <param name="enable_dc">false</param>
     <param name="axes_names">( "joint1" )</param>
@@ -17,4 +18,7 @@ BSD-3-Clause license. -->
     <param name="velocity_feedback_joint"> ( "606C" )</param>
     <param name="position_window_deg"> ( 0.1 )</param>
     <param name="timing_window_ms"> ( 100 )</param>
+    <param name="pos_limit_min_deg">               (        -23.0       )</param>
+    <param name="pos_limit_max_deg">               (         23.0       )</param>
+    <param name="use_position_limits_from_config"> (         false      )</param>
 </device>

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<int32_t>(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<bool>& 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;
     {

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.