Add basic pixi support and add basic smoke test of wholebodydynamics device

.gitattributes

@@ -0,0 +1,3 @@
+# GitHub syntax highlighting
+pixi.lock linguist-language=YAML
+

.github/workflows/conda-ci.yml

@@ -41,7 +41,7 @@ jobs:
       run: |
         mkdir -p build
         cd build
-        cmake -GNinja -DBUILD_TESTING:BOOL=ON \
+        cmake -GNinja -DBUILD_TESTING:BOOL=OFF \
               -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} ..
 
     - name: Configure [Windows]
@@ -50,7 +50,7 @@ jobs:
       run: |
         mkdir -p build
         cd build
-        cmake -G"Visual Studio 16 2019" -DBUILD_TESTING:BOOL=ON \
+        cmake -G"Visual Studio 16 2019" -DBUILD_TESTING:BOOL=OFF \
               -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} ..
 
     - name: Build

.github/workflows/pixi-ci.yml

@@ -0,0 +1,31 @@
+name: CI Workflow
+
+on:
+  pull_request:
+
+jobs:
+  build-with-pixi:
+    name: '[pixi:${{ matrix.os }}]'
+    runs-on: ${{ matrix.os }}
+    strategy:
+      fail-fast: false
+      matrix:
+        build_type: [Release]
+        os: [ubuntu-22.04, macos-latest, windows-2019]
+
+    steps:
+    - uses: actions/checkout@v4
+
+    - name: Print used environment
+      shell: bash
+      run: |
+        env
+
+    - uses: prefix-dev/setup-pixi@v0.5.1
+
+    - name: Run the tests
+      shell: bash -l {0}
+      run: |
+        pixi run download_deps
+        pixi run install_deps
+        pixi run test

.gitignore

@@ -37,3 +37,6 @@
 # clangd
 **/compile_commands.json
 **/.cache/
+# pixi environments
+.pixi*
+.build*

CMakeLists.txt

@@ -30,11 +30,17 @@ add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BIND
 
 set(YARP_REQUIRED_VERSION 3.0.1)
 
-find_package(YARP ${YARP_REQUIRED_VERSION} REQUIRED)
+find_package(YARP ${YARP_REQUIRED_VERSION} REQUIRED COMPONENTS robotinterface idl_tools)
 find_package(Eigen3 3.2.92 REQUIRED)
 
 yarp_configure_plugins_installation(${PROJECT_NAME})
 
+option(BUILD_TESTING "Create tests using CMake" OFF)
+if(BUILD_TESTING)
+    find_package(Catch2 3.0.1 REQUIRED)
+    enable_testing()
+endif()
+
 add_subdirectory(libraries)
 add_subdirectory(idl)
 add_subdirectory(devices)

README.md

@@ -4,7 +4,9 @@ YARP-based estimators for humanoid robots.
 # Overview
  - [:orange_book: Implementation](#orange_book-implementation)
  - [:page_facing_up: Dependencies](#page_facing_up-dependencies)
- - [:hammer: Build the suite](#hammer-build-the-suite)
+ - [:hammer: Installation](#installation)
+ - [Usage](#usage)
+ - [Authors](#authors)
 
 # :orange_book: Implementation
 The current implementations available in the `devices` folder include,
@@ -27,8 +29,37 @@ The current implementations available in the `devices` folder include,
 
 It must be noted that all of these dependencies can be directly installed together in one place using the [robotology-superbuild](https://github.com/robotology/robotology-superbuild), in particular enabling its `Dynamics` component. 
 
-# :hammer: Build the suite
-## Linux
+# :hammer: Installation
+
+## conda (recommended) 
+
+You can easily install `whole-body-estimators` with with `conda` using the following command
+~~~
+conda install -c conda-forge -c robotology whole-body-estimators
+~~~
+
+If you are not familiar with conda or conda-forge, you can read an introduction document in [conda-forge overview](https://github.com/robotology/robotology-superbuild/blob/master/doc/conda-forge.md#conda-forge-overview).
+
+## robotology-superbuild (advanced)
+
+If you are installing `whole-body-estimators` for use as part of [iCub humanoid robot software installation](https://icub-tech-iit.github.io/documentation/sw_installation/), you may want to install `whole-body-estimators` through the [robotology-superbuild](https://github.com/robotology/robotology-superbuild), an easy way to download, compile and install the robotology software on multiple operating systems, using the [CMake](https://www.cmake.org) build system and its extension [YCM](http://robotology.github.io/ycm). To get `whole-body-estimators` when using the `robotology-superbuild`, please enable the `ROBOTOLOGY_ENABLE_DYNAMICS` CMake option of the superbuild. 
+
+
+## Build from source with pixi (advanced)
+
+If you are just interesting in building the devices to check that the compilation is working fine and tests pass, you can use pixi:
+
+~~~
+git clone https://github.com/robotology/whole-body-estimators.git
+cd whole-body-estimators
+pixi run download_deps
+pixi run install_deps
+pixi run test
+~~~
+
+## Build from source manually (advanced)
+
+### Linux
 
 ```sh
 git clone https://github.com/robotology/whole-body-estimators.git
@@ -38,18 +69,21 @@ cmake ../
 make
 [sudo] make install
 ```
+
 Notice: `sudo` is not necessary if you specify the `CMAKE_INSTALL_PREFIX`. In this case it is necessary to add in the `.bashrc` or `.bash_profile` the following lines:
 ``` sh
 export WBDEstimator_INSTALL_DIR=/path/where/you/installed
 export YARP_DATA_DIRS=${YARP_DATA_DIRS}:${WBDEstimator_INSTALL_DIR}/share/yarp
 ```
 Note that this is not necessary if you install `whole-body-estimators` via the `robotology-superbuild` .
 
-# Using the estimators
+# Usage
+
 - [`baseEstimatorV1`](devices/baseEstimatorV1/README.md) Please follow the documentation available here to run the floating base estimator.
 - [`wholeBodyDynamics`](devices/wholeBodyDynamics/README.md) Please follow the documentation for a description of features/parameters of the device and please follow the documentation in [Force Control on iCub](doc/howto/force_control_on_icub.md) for running the whole body dynamics device.
 
 # Authors
+
 ```
 Hosameldin Awadalla Omer Mohamed <hosameldin.mohamed@iit.it>
 Francisco Javier Andrade Chavez <FranciscoJavier.AndradeChavez@iit.it>

cmake/WBEAddUnitTest.cmake

@@ -0,0 +1,25 @@
+function(wbe_add_unit_test)
+    set(options )
+    set(oneValueArgs NAME)
+    set(multiValueArgs SOURCES LINKS)
+
+    set(prefix "wbe_add_unit_test")
+
+    cmake_parse_arguments(${prefix}
+        "${options}"
+        "${oneValueArgs}"
+        "${multiValueArgs}"
+        ${ARGN})
+
+    set(name ${${prefix}_NAME})
+    set(unit_test_files ${${prefix}_SOURCES})
+
+    set(targetname ${name}UnitTests)
+    add_executable(${targetname}
+        "${unit_test_files}")
+
+    target_link_libraries(${targetname} PRIVATE Catch2::Catch2WithMain ${${prefix}_LINKS})
+    target_compile_definitions(${targetname} PRIVATE CATCH_CONFIG_FAST_COMPILE CATCH_CONFIG_DISABLE_MATCHERS)
+
+    add_test(NAME ${targetname} COMMAND ${targetname})
+endfunction()

devices/wholeBodyDynamics/CMakeLists.txt

@@ -47,4 +47,8 @@ if(ENABLE_wholebodydynamics)
 
     # Install configuration files
     add_subdirectory(app)
+
+    if(BUILD_TESTING)
+        add_subdirectory(test)
+    endif()
 endif()

devices/wholeBodyDynamics/test/CMakeLists.txt

@@ -0,0 +1,11 @@
+include(WBEAddUnitTest)
+
+add_subdirectory(fakeFTs)
+
+wbe_add_unit_test(NAME WholeBodyDynamics
+                  SOURCES WholeBodyDynamicsUnitTests.cpp
+                  LINKS YARP::YARP_robotinterface)
+
+target_compile_definitions(WholeBodyDynamicsUnitTests 
+                           PRIVATE CMAKE_CURRENT_SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}"
+                                   PROJECT_BINARY_DIR="${PROJECT_BINARY_DIR}")

devices/wholeBodyDynamics/test/WholeBodyDynamicsUnitTests.cpp

@@ -0,0 +1,62 @@
+// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
+// SPDX-License-Identifier: BSD-3-Clause
+
+#include <catch2/catch_test_macros.hpp>
+
+// yarprobotinterface
+#include <yarp/os/Network.h>
+#include <yarp/robotinterface/XMLReader.h>
+#include <yarp/dev/PolyDriver.h>
+
+#include <cstdlib>
+#include <iostream>
+
+TEST_CASE("WholeBodyDynamicsSmokeTest")
+{
+    // Add PROJECT_BINARY_DIR/share to YARP_DATA_DIRS to find fakeFTs and wholebodydynamics and fakeFTs devices
+    // from the build directory
+    std::string oldyarpdatadirs = getenv("YARP_DATA_DIRS");
+
+    #ifdef _WIN32
+    std::string pathsep = ";";
+    #else
+    std::string pathsep = ":";
+    #endif
+    std::string newyarpdatadirs = std::string("YARP_DATA_DIRS=")+std::string(PROJECT_BINARY_DIR)+"/share/yarp"+pathsep+oldyarpdatadirs;
+    #ifdef _WIN32
+    _putenv(newyarpdatadirs.c_str());
+    #else
+    putenv(const_cast<char*>(newyarpdatadirs.c_str()));
+    #endif
+
+    // Set YARP_ROBOT_NAME to ensure that the correct ergocub file is found by wholebodydynamics
+    std::string newyarprobotname = std::string("YARP_ROBOT_NAME=ergoCubSN001");
+    #ifdef _WIN32
+    _putenv(newyarprobotname.c_str());
+    #else
+    putenv(const_cast<char*>(newyarprobotname.c_str()));
+    #endif
+
+    // Avoid to need yarpserver
+    yarp::os::NetworkBase::setLocalMode(true);
+
+    // Load the yarprobotinterface ergocub_test_all.xml file, and check for errors
+    yarp::os::Network yarpInit;
+    yarp::robotinterface::XMLReader xmlRobotInterfaceReader;
+    yarp::robotinterface::XMLReaderResult xmlRobotInterfaceResult;
+    std::string location_ergocub_test_all = std::string(CMAKE_CURRENT_SOURCE_DIR) + std::string("/ergocub_test_all.xml");
+
+    xmlRobotInterfaceResult = xmlRobotInterfaceReader.getRobotFromFile(location_ergocub_test_all);
+
+    REQUIRE(xmlRobotInterfaceResult.parsingIsSuccessful);
+
+    // Enter the startup phase, that will open all the devices and  call attach if necessary
+    REQUIRE(xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseStartup));
+    REQUIRE(xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseRun));
+
+    // If the robotinterface started, we are good to go, we can close it 
+    REQUIRE(xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseInterrupt1));
+    REQUIRE(xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseInterrupt2));
+    REQUIRE(xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseInterrupt3));
+    REQUIRE(xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseShutdown));
+}

devices/wholeBodyDynamics/test/ergocub_simulated_wholebodydynamics.xml

@@ -0,0 +1,54 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="wholebodydynamics" type="wholebodydynamics">
+  <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
+  <param name="modelFile">model.urdf</param>
+  <param name="fixedFrameGravity">(0,0,-9.81)</param>
+  <param name="defaultContactFrames">(l_hand_palm,r_hand_palm,root_link,l_foot_front,l_foot_rear,r_foot_front,r_foot_rear,l_upper_leg,r_upper_leg)</param>
+  <param name="imuFrameName">waist_imu_0</param>
+  <param name="publishOnROS">true</param>
+  <param name="forceTorqueEstimateConfidence">2</param>
+  <param name="useJointVelocity">true</param>
+  <param name="useJointAcceleration">false</param>
+  <param name="streamFilteredFT">true</param>
+  <param name="imuFilterCutoffInHz">3.0</param>
+  <param name="forceTorqueFilterCutoffInHz">3.0</param>
+  <param name="jointVelFilterCutoffInHz">3.0</param> <!-- used if useJointVelocity is set to true -->
+  <param name="jointAccFilterCutoffInHz">3.0</param> <!-- used if useJointAcceleration is set to true -->
+  <param name="devicePeriodInSeconds">0.002</param>
+  <param name="useSkinForContacts">false</param>
+  <param name="publishNetExternalWrenches">true</param>
+  <!--<param name="assume_fixed">root_link</param>-->
+  <group name="HW_USE_MAS_IMU">
+    <param name="accelerometer">waist_imu_0</param>
+    <param name="gyroscope">waist_imu_0</param>
+  </group>
+
+  <group name="multipleAnalogSensorsNames">
+    <param name="SixAxisForceTorqueSensorsNames">(l_leg_ft, l_foot_front_ft, l_foot_rear_ft, r_leg_ft, r_foot_front_ft, r_foot_rear_ft, r_arm_ft, l_arm_ft)</param>
+  </group>
+
+  <group name="GRAVITY_COMPENSATION">
+    <param name="enableGravityCompensation">true</param>
+    <param name="gravityCompensationBaseLink">root_link</param>
+    <param name="gravityCompensationAxesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch,neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
+  </group>
+
+  <action phase="startup" level="15" type="attach">
+    <paramlist name="networks">
+      <!-- motorcontrol -->
+      <elem name="controlboard">simulated_controlboard</elem>
+
+      <!-- imu -->
+      <elem name="imu">simulated_imu</elem>
+
+      <!-- fts -->
+      <elem name="fts">simulated_fts</elem>
+    </paramlist>
+  </action>
+
+  <action phase="shutdown" level="2" type="detach" />
+
+</device>

devices/wholeBodyDynamics/test/ergocub_test_all.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<robot name="ergoCubSN001" build="0" portprefix="/ergocub" xmlns:xi="http://www.w3.org/2001/XInclude">
+  <devices>
+    <!-- Simulated controlboards -->
+    <device name="simulated_controlboard" type="fakeMotionControl">
+        <group name="GENERAL">
+            <param name="Joints">24</param>
+            <param name="AxisName">(torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
+        </group>
+    </device>
+
+    <!-- Simulated FT sensors -->
+    <device name="simulated_fts" type="fakeFTs">
+        <param name="sensorNames">(l_leg_ft, l_foot_front_ft, l_foot_rear_ft, r_leg_ft, r_foot_front_ft, r_foot_rear_ft, r_arm_ft, l_arm_ft)</param>
+    </device>
+
+    <!-- Simulated IMUs -->
+    <device name="simulated_imu" type="fakeIMU">
+        <param name="sensorName">"waist_imu_0"</param>
+        <param name="frameName">"waist_imu_0"</param>
+    </device>
+
+    <!-- Simulated WBD -->
+    <xi:include href="./ergocub_simulated_wholebodydynamics.xml" />
+  </devices>
+</robot>

devices/wholeBodyDynamics/test/fakeFTs/CMakeLists.txt

@@ -0,0 +1,29 @@
+yarp_prepare_plugin(fakeFTs
+  CATEGORY device
+  TYPE fakeFTs
+  INCLUDE fakeFTs.h
+  DEFAULT ON
+  INTERNAL
+  ADVANCED
+)
+
+yarp_add_plugin(fakeFTs)
+target_sources(fakeFTs
+  PRIVATE
+    fakeFTs.cpp
+    fakeFTs.h
+  )
+target_link_libraries(fakeFTs
+  PRIVATE
+    YARP::YARP_os
+    YARP::YARP_sig
+    YARP::YARP_dev
+    YARP::YARP_math
+)
+
+yarp_install(TARGETS fakeFTs
+             EXPORT Runtime
+             COMPONENT runtime
+             LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR}
+             ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}
+             YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR})