Update CHANGELOG

CHANGELOG.md

@@ -13,6 +13,7 @@ All notable changes to this project are documented in this file.
 - Add the possibility to control a subset of linear coordinates in `TSID::SE3Task` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/738)
 - Implement `GlobalCoPEvaluator` in `Contacts` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/745)
 - Implement `Wrench::getLocalCoP()` method (https://github.com/ami-iit/bipedal-locomotion-framework/pull/745)
+- Add tests for classes of `RobotDynamicsEstimator` library (https://github.com/ami-iit/bipedal-locomotion-framework/pull/743)
 
 ### Changed
 - Remove the possibility to disable the telemetry in `System::AdvanceableRunner` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/726)

src/Estimators/CMakeLists.txt

@@ -36,6 +36,7 @@ if(FRAMEWORK_COMPILE_RobotDynamicsEstimator)
                             ${H_PREFIX}/GyroscopeMeasurementDynamics.h ${H_PREFIX}/FrictionTorqueStateDynamics.h ${H_PREFIX}/MotorCurrentMeasurementDynamics.h
     PUBLIC_LINK_LIBRARIES   BipedalLocomotion::ParametersHandler MANIF::manif BipedalLocomotion::System BipedalLocomotion::Math iDynTree::idyntree-high-level
                             iDynTree::idyntree-core iDynTree::idyntree-model iDynTree::idyntree-modelio Eigen3::Eigen BayesFilters::BayesFilters
-    PRIVATE_LINK_LIBRARIES  BipedalLocomotion::TextLogging BipedalLocomotion::ManifConversions
+                            BipedalLocomotion::ManifConversions
+    PRIVATE_LINK_LIBRARIES  BipedalLocomotion::TextLogging
     )
 endif()

src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h

@@ -56,7 +56,7 @@ class AccelerometerMeasurementDynamics : public Dynamics
                                                         and the sizes in the ukf state vector. */
     Eigen::VectorXd m_covSingleVar; /**< Covariance of the accelerometer measurement from
                                        configuration. */
-    Eigen::VectorXd m_subModelBaseAcceleration; /**< Base acceleration of the sub-model. */
+    manif::SE3d::Tangent m_subModelBaseAcceleration; /**< Base acceleration of the sub-model. */
     manif::SE3d::Tangent m_accelerometerFameVelocity; /** Velocity of the accelerometer given by the forward dynamics. */
     manif::SE3d::Tangent m_accelerometerFameAcceleration; /** Acceleration of the accelerometer given by the forward dynamics. */
     manif::SO3d m_imuRworld; /**< Rotation matrix of the inertial frame with respect to the imu frame expressed in the imu frame. */

src/Estimators/src/AccelerometerMeasurementDynamics.cpp

@@ -191,7 +191,8 @@ bool RDE::AccelerometerMeasurementDynamics::update()
 
         if (!m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]
                  ->getFrameAcc(m_accelerometerFrameName,
-                               m_subModelBaseAcceleration,
+                               iDynTree::make_span(m_subModelBaseAcceleration.data(),
+                                                   manif::SE3d::Tangent::DoF),
                                m_subModelJointAcc[m_subModelsWithAccelerometer[index]],
                                iDynTree::make_span(m_accelerometerFameAcceleration.data(),
                                                    manif::SE3d::Tangent::DoF)))

src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp

@@ -8,7 +8,6 @@
 #include <catch2/catch_test_macros.hpp>
 #include <catch2/generators/catch_generators_all.hpp>
 
-#include <ConfigFolderPath.h>
 #include <iCubModels/iCubModels.h>
 #include <yarp/os/ResourceFinder.h>
 
@@ -22,7 +21,6 @@
 #include <BipedalLocomotion/Math/Constants.h>
 #include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
 #include <BipedalLocomotion/ParametersHandler/StdImplementation.h>
-#include <BipedalLocomotion/ParametersHandler/YarpImplementation.h>
 #include <BipedalLocomotion/System/VariablesHandler.h>
 
 #include <BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h>
@@ -151,9 +149,7 @@ Eigen::Ref<Eigen::VectorXd> createStateVector(UKFInput& input,
                                               VariablesHandler& stateVariableHandler,
                                               std::shared_ptr<iDynTree::KinDynComputations> kinDyn)
 {
-    Eigen::VectorXd state = Eigen::VectorXd(stateVariableHandler.getNumberOfVariables());
-
-    state.setZero();
+    Eigen::VectorXd state = Eigen::VectorXd::Zero(stateVariableHandler.getNumberOfVariables());
 
     Eigen::VectorXd jointVel = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size);
 
@@ -250,16 +246,16 @@ void setRandomKinDynState(std::vector<SubModel>& subModelList,
     offset = stateVariableHandler.getVariable("tau_m").offset;
     size = stateVariableHandler.getVariable("tau_m").size;
 
-    Eigen::VectorXd jointTrq = Eigen::VectorXd(size);
+    Eigen::VectorXd jointTrq(size);
     for (int jointIndex = 0; jointIndex < size; jointIndex++)
     {
         jointTrq(jointIndex) = state[offset + jointIndex];
     }
 
-    Eigen::VectorXd jointAcc = Eigen::VectorXd(size);
+    Eigen::VectorXd jointAcc(size);
     REQUIRE(kinDynWrapperList[0]->forwardDynamics(jointTrq,
-                                                  Eigen::VectorXd().Zero(size),
-                                                  Eigen::VectorXd().Zero(size),
+                                                  Eigen::VectorXd::Zero(size),
+                                                  Eigen::VectorXd::Zero(size),
                                                   input.robotBaseAcceleration,
                                                   jointAcc));
 }
@@ -341,7 +337,8 @@ TEST_CASE("Accelerometer Measurement Dynamics")
 
     manif::SE3Tangentd accelerometerFameAcceleration;
     REQUIRE(kinDyn->getFrameAcc("r_leg_ft",
-                                input.robotBaseAcceleration,
+                                iDynTree::make_span(input.robotBaseAcceleration.data(),
+                                                    manif::SE3d::Tangent::DoF),
                                 input.robotJointAccelerations,
                                 iDynTree::make_span(accelerometerFameAcceleration.data(),
                                                     manif::SE3d::Tangent::DoF)));

src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt

@@ -2,89 +2,76 @@
 # This software may be modified and distributed under the terms of the
 # GNU Lesser General Public License v2.1 or any later version.
 
-if(FRAMEWORK_USE_icub-models AND FRAMEWORK_COMPILE_YarpImplementation)
+add_bipedal_test(NAME ZeroVelocityStateDynamics
+                SOURCES ZeroVelocityStateDynamicsTest.cpp
+                LINKS BipedalLocomotion::RobotDynamicsEstimator
+                BipedalLocomotion::ParametersHandler
+                Eigen3::Eigen)
 
-    add_bipedal_test(NAME SubModelCreator
-        SOURCES SubModelCreatorTest.cpp
-        LINKS BipedalLocomotion::RobotDynamicsEstimator
-        BipedalLocomotion::ParametersHandler
-        BipedalLocomotion::ParametersHandlerYarpImplementation
-        icub-models::icub-models)
+add_bipedal_test(NAME ConstantMeasurementModel
+                SOURCES ConstantMeasurementModelTest.cpp
+                LINKS BipedalLocomotion::RobotDynamicsEstimator
+                BipedalLocomotion::ParametersHandler
+                Eigen3::Eigen)
 
-    add_bipedal_test(NAME KinDynWrapper
-        SOURCES KinDynWrapperTest.cpp
-        LINKS BipedalLocomotion::RobotDynamicsEstimator
-        BipedalLocomotion::ParametersHandler
-        BipedalLocomotion::ParametersHandlerYarpImplementation
-        icub-models::icub-models)
+if(FRAMEWORK_USE_icub-models)
+    if(FRAMEWORK_COMPILE_YarpImplementation)
+        include_directories(${CMAKE_CURRENT_BINARY_DIR})
+        configure_file("${CMAKE_CURRENT_SOURCE_DIR}/ConfigFolderPath.h.in" "${CMAKE_CURRENT_BINARY_DIR}/ConfigFolderPath.h" @ONLY)
 
-    add_bipedal_test(NAME ZeroVelocityStateDynamics
-        SOURCES ZeroVelocityStateDynamicsTest.cpp
-        LINKS BipedalLocomotion::RobotDynamicsEstimator
-        BipedalLocomotion::ParametersHandler
-        BipedalLocomotion::ParametersHandlerYarpImplementation
-        icub-models::icub-models
-        Eigen3::Eigen
-        BipedalLocomotion::ManifConversions)
+        add_bipedal_test(NAME SubModelCreator
+            SOURCES SubModelCreatorTest.cpp
+            LINKS BipedalLocomotion::RobotDynamicsEstimator
+            BipedalLocomotion::ParametersHandler
+            BipedalLocomotion::ParametersHandlerYarpImplementation
+            icub-models::icub-models)
+    endif()
 
-    add_bipedal_test(NAME ConstantMeasurementModel
-        SOURCES ConstantMeasurementModelTest.cpp
-        LINKS BipedalLocomotion::RobotDynamicsEstimator
-        BipedalLocomotion::ParametersHandler
-        BipedalLocomotion::ParametersHandlerYarpImplementation
-        icub-models::icub-models
-        Eigen3::Eigen
-        BipedalLocomotion::ManifConversions)
+    if (FRAMEWORK_COMPILE_ManifConversions)
+        add_bipedal_test(NAME KinDynWrapper
+            SOURCES KinDynWrapperTest.cpp
+            LINKS BipedalLocomotion::RobotDynamicsEstimator
+            BipedalLocomotion::ParametersHandler
+            icub-models::icub-models
+            BipedalLocomotion::ManifConversions)
 
-    add_bipedal_test(NAME AccelerometerMeasurementDynamics
-        SOURCES AccelerometerMeasurementDynamicsTest.cpp
-        LINKS   BipedalLocomotion::RobotDynamicsEstimator
-        BipedalLocomotion::ParametersHandler
-        BipedalLocomotion::ParametersHandlerYarpImplementation
-        BipedalLocomotion::System
-        icub-models::icub-models
-        Eigen3::Eigen
-        MANIF::manif
-        BipedalLocomotion::ManifConversions
-        ${iDynTree_LIBRARIES})
+        add_bipedal_test(NAME FrictionTorqueStateDynamics
+            SOURCES FrictionTorqueStateDynamicsTest.cpp
+            LINKS BipedalLocomotion::RobotDynamicsEstimator
+            BipedalLocomotion::ParametersHandler
+            icub-models::icub-models
+            Eigen3::Eigen
+            BipedalLocomotion::ManifConversions
+            iDynTree::idyntree-model)
 
-    add_bipedal_test(NAME FrictionTorqueStateDynamics
-        SOURCES FrictionTorqueStateDynamicsTest.cpp
-        LINKS BipedalLocomotion::RobotDynamicsEstimator
-        BipedalLocomotion::ParametersHandler
-        BipedalLocomotion::ParametersHandlerYarpImplementation
-        BipedalLocomotion::System
-        icub-models::icub-models
-        Eigen3::Eigen
-        MANIF::manif
-        BipedalLocomotion::ManifConversions
-        ${iDynTree_LIBRARIES})
+        add_bipedal_test(NAME GyroscopeMeasurementDynamics
+            SOURCES GyroscopeMeasurementDynamicsTest.cpp
+            LINKS BipedalLocomotion::RobotDynamicsEstimator
+            BipedalLocomotion::ParametersHandler
+            icub-models::icub-models
+            Eigen3::Eigen
+            BipedalLocomotion::ManifConversions
+            iDynTree::idyntree-model)
 
-    add_bipedal_test(NAME GyroscopeMeasurementDynamics
-        SOURCES GyroscopeMeasurementDynamicsTest.cpp
-        LINKS BipedalLocomotion::RobotDynamicsEstimator
-        BipedalLocomotion::ParametersHandler
-        BipedalLocomotion::ParametersHandlerYarpImplementation
-        BipedalLocomotion::System
-        icub-models::icub-models
-        Eigen3::Eigen
-        MANIF::manif
-        BipedalLocomotion::ManifConversions
-        ${iDynTree_LIBRARIES})
+        add_bipedal_test(NAME AccelerometerMeasurementDynamics
+            SOURCES AccelerometerMeasurementDynamicsTest.cpp
+            LINKS   BipedalLocomotion::RobotDynamicsEstimator
+            BipedalLocomotion::ParametersHandler
+            icub-models::icub-models
+            Eigen3::Eigen
+            MANIF::manif
+            BipedalLocomotion::ManifConversions
+            iDynTree::idyntree-model)
 
-    add_bipedal_test(NAME JointVelocityStateDynamics
-        SOURCES JointVelocityStateDynamicsTest.cpp
-        LINKS BipedalLocomotion::RobotDynamicsEstimator
-        BipedalLocomotion::ParametersHandler
-        BipedalLocomotion::ParametersHandlerYarpImplementation
-        BipedalLocomotion::System
-        icub-models::icub-models
-        Eigen3::Eigen
-        MANIF::manif
-        BipedalLocomotion::ManifConversions
-        ${iDynTree_LIBRARIES})
-
-    include_directories(${CMAKE_CURRENT_BINARY_DIR})
-    configure_file("${CMAKE_CURRENT_SOURCE_DIR}/ConfigFolderPath.h.in" "${CMAKE_CURRENT_BINARY_DIR}/ConfigFolderPath.h" @ONLY)
+        add_bipedal_test(NAME JointVelocityStateDynamics
+            SOURCES JointVelocityStateDynamicsTest.cpp
+            LINKS BipedalLocomotion::RobotDynamicsEstimator
+            BipedalLocomotion::ParametersHandler
+            icub-models::icub-models
+            Eigen3::Eigen
+            MANIF::manif
+            BipedalLocomotion::ManifConversions
+            iDynTree::idyntree-model)
+    endif()
 
 endif()

src/Estimators/tests/RobotDynamicsEstimator/ConstantMeasurementModelTest.cpp

@@ -8,12 +8,8 @@
 #include <catch2/catch_test_macros.hpp>
 #include <catch2/generators/catch_generators_all.hpp>
 
-#include <ConfigFolderPath.h>
-#include <iCubModels/iCubModels.h>
 #include <yarp/os/ResourceFinder.h>
 
-#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
-#include <BipedalLocomotion/ParametersHandler/YarpImplementation.h>
 #include <BipedalLocomotion/ParametersHandler/StdImplementation.h>
 
 #include <BipedalLocomotion/RobotDynamicsEstimator/ConstantMeasurementModel.h>
@@ -36,9 +32,7 @@ TEST_CASE("Constant Measurement Model")
     REQUIRE(variableHandler.addVariable("r_leg_ft_bias", sizeVariable));
     REQUIRE(variableHandler.addVariable("r_foot_front_ft", sizeVariable));
 
-    Eigen::VectorXd state;
-    state.resize(sizeVariable * variableHandler.getNumberOfVariables());
-    state.setZero();
+    Eigen::VectorXd state = Eigen::VectorXd::Zero(sizeVariable * variableHandler.getNumberOfVariables());
 
     const std::string name = "r_leg_ft";
     Eigen::VectorXd covariance(6);

src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp

@@ -9,7 +9,6 @@
 #include <catch2/generators/catch_generators_all.hpp>
 #include <chrono>
 
-#include <ConfigFolderPath.h>
 #include <iCubModels/iCubModels.h>
 #include <yarp/os/ResourceFinder.h>
 
@@ -20,7 +19,6 @@
 #include <BipedalLocomotion/Math/Constants.h>
 #include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
 #include <BipedalLocomotion/ParametersHandler/StdImplementation.h>
-#include <BipedalLocomotion/ParametersHandler/YarpImplementation.h>
 #include <BipedalLocomotion/System/VariablesHandler.h>
 
 #include <BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h>
@@ -143,9 +141,7 @@ Eigen::Ref<Eigen::VectorXd> createStateVector(UKFInput& input,
                                               std::shared_ptr<iDynTree::KinDynComputations> kinDyn,
                                               IParametersHandler::shared_ptr frictionParamHandler)
 {
-    Eigen::VectorXd state = Eigen::VectorXd(stateVariableHandler.getNumberOfVariables());
-
-    state.setZero();
+    Eigen::VectorXd state = Eigen::VectorXd::Zero(stateVariableHandler.getNumberOfVariables());
 
     Eigen::VectorXd jointVel = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size);
 
@@ -231,7 +227,7 @@ void computeTauFNext(UKFInput& input,
                      IParametersHandler::shared_ptr frictionParamHandler,
                      Eigen::Ref<Eigen::VectorXd> tauFNext)
 {
-    Eigen::VectorXd jointVel = Eigen::VectorXd(stateVariableHandler.getVariable("ds").size);
+    Eigen::VectorXd jointVel(stateVariableHandler.getVariable("ds").size);
 
     int offsetVel = stateVariableHandler.getVariable("ds").offset;
 
@@ -314,7 +310,7 @@ TEST_CASE("Friction Torque Dynamics")
     tauFDynamics.setInput(input);
     tauFDynamics.setState(state);
 
-    Eigen::VectorXd tauFNext = Eigen::VectorXd(input.robotJointPositions.size());
+    Eigen::VectorXd tauFNext(input.robotJointPositions.size());
     computeTauFNext(input, state, stateVariableHandler, frictionParameterHandler, tauFNext);
 
     // Update the dynamics

src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp

@@ -8,7 +8,6 @@
 #include <catch2/catch_test_macros.hpp>
 #include <catch2/generators/catch_generators_all.hpp>
 
-#include <ConfigFolderPath.h>
 #include <iCubModels/iCubModels.h>
 #include <yarp/os/ResourceFinder.h>
 
@@ -22,7 +21,6 @@
 #include <BipedalLocomotion/Math/Constants.h>
 #include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
 #include <BipedalLocomotion/ParametersHandler/StdImplementation.h>
-#include <BipedalLocomotion/ParametersHandler/YarpImplementation.h>
 #include <BipedalLocomotion/System/VariablesHandler.h>
 
 #include <BipedalLocomotion/RobotDynamicsEstimator/GyroscopeMeasurementDynamics.h>
@@ -151,9 +149,7 @@ Eigen::Ref<Eigen::VectorXd> createStateVector(UKFInput& input,
                                               VariablesHandler& stateVariableHandler,
                                               std::shared_ptr<iDynTree::KinDynComputations> kinDyn)
 {
-    Eigen::VectorXd state = Eigen::VectorXd(stateVariableHandler.getNumberOfVariables());
-
-    state.setZero();
+    Eigen::VectorXd state = Eigen::VectorXd::Zero(stateVariableHandler.getNumberOfVariables());
 
     Eigen::VectorXd jointVel = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size);
 
@@ -205,7 +201,7 @@ void setRandomKinDynState(std::vector<SubModel>& subModelList,
     int offset = stateVariableHandler.getVariable("ds").offset;
     int size = stateVariableHandler.getVariable("ds").size;
 
-    Eigen::VectorXd jointVel = Eigen::VectorXd(size);
+    Eigen::VectorXd jointVel(size);
     for (int jointIndex = 0; jointIndex < size; jointIndex++)
     {
         jointVel(jointIndex) = state[offset + jointIndex];

src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp

@@ -9,7 +9,6 @@
 #include <catch2/generators/catch_generators_all.hpp>
 #include <chrono>
 
-#include <ConfigFolderPath.h>
 #include <iCubModels/iCubModels.h>
 #include <yarp/os/ResourceFinder.h>
 
@@ -23,7 +22,6 @@
 #include <BipedalLocomotion/Math/Constants.h>
 #include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
 #include <BipedalLocomotion/ParametersHandler/StdImplementation.h>
-#include <BipedalLocomotion/ParametersHandler/YarpImplementation.h>
 #include <BipedalLocomotion/System/VariablesHandler.h>
 
 #include <BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h>
@@ -152,9 +150,7 @@ Eigen::Ref<Eigen::VectorXd> createStateVector(UKFInput& input,
                                               VariablesHandler& stateVariableHandler,
                                               std::shared_ptr<iDynTree::KinDynComputations> kinDyn)
 {
-    Eigen::VectorXd state = Eigen::VectorXd(stateVariableHandler.getNumberOfVariables());
-
-    state.setZero();
+    Eigen::VectorXd state = Eigen::VectorXd::Zero(stateVariableHandler.getNumberOfVariables());
 
     Eigen::VectorXd jointVel = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size);