diff --git a/CHANGELOG.md b/CHANGELOG.md index 57d2a3b355..07df8431d5 100644 --- a/CHANGELOG.md +++ b/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) diff --git a/src/Estimators/CMakeLists.txt b/src/Estimators/CMakeLists.txt index 74820fdbe3..daa11440e8 100644 --- a/src/Estimators/CMakeLists.txt +++ b/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() diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h index 40855799a6..1f67c55197 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h +++ b/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. */ diff --git a/src/Estimators/src/AccelerometerMeasurementDynamics.cpp b/src/Estimators/src/AccelerometerMeasurementDynamics.cpp index 6c59dbb8f2..038ef6f249 100644 --- a/src/Estimators/src/AccelerometerMeasurementDynamics.cpp +++ b/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))) diff --git a/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp index 35e24fc425..8f9e4e8a07 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp @@ -8,7 +8,6 @@ #include #include -#include #include #include @@ -22,7 +21,6 @@ #include #include #include -#include #include #include @@ -151,9 +149,7 @@ Eigen::Ref createStateVector(UKFInput& input, VariablesHandler& stateVariableHandler, std::shared_ptr 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& 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))); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt b/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt index 0cb30d7cc6..a729609688 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt +++ b/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() diff --git a/src/Estimators/tests/RobotDynamicsEstimator/ConstantMeasurementModelTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/ConstantMeasurementModelTest.cpp index 916f052377..6cabfdca91 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/ConstantMeasurementModelTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/ConstantMeasurementModelTest.cpp @@ -8,12 +8,8 @@ #include #include -#include -#include #include -#include -#include #include #include @@ -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); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp index 3b952efb6a..24076ec1fd 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp @@ -9,7 +9,6 @@ #include #include -#include #include #include @@ -20,7 +19,6 @@ #include #include #include -#include #include #include @@ -143,9 +141,7 @@ Eigen::Ref createStateVector(UKFInput& input, std::shared_ptr 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 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 diff --git a/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp index 2d8a0c9594..129c04515f 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp @@ -8,7 +8,6 @@ #include #include -#include #include #include @@ -22,7 +21,6 @@ #include #include #include -#include #include #include @@ -151,9 +149,7 @@ Eigen::Ref createStateVector(UKFInput& input, VariablesHandler& stateVariableHandler, std::shared_ptr 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& 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]; diff --git a/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp index 9ebbd04643..05ad718254 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp @@ -9,7 +9,6 @@ #include #include -#include #include #include @@ -23,7 +22,6 @@ #include #include #include -#include #include #include @@ -152,9 +150,7 @@ Eigen::Ref createStateVector(UKFInput& input, VariablesHandler& stateVariableHandler, std::shared_ptr 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); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/KinDynWrapperTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/KinDynWrapperTest.cpp index 68d4ca1e4e..47b74b0fff 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/KinDynWrapperTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/KinDynWrapperTest.cpp @@ -8,7 +8,6 @@ #include #include -#include #include #include @@ -22,7 +21,6 @@ #include #include #include -#include #include #include @@ -66,7 +64,7 @@ bool setStaticState(std::shared_ptr kinDyn) { size_t dofs = kinDyn->getNrOfDegreesOfFreedom(); iDynTree::Transform worldTbase; - Eigen::VectorXd baseVel(6); + manif::SE3Tangentd baseVel; Eigen::Vector3d gravity; Eigen::VectorXd qj(dofs), dqj(dofs), ddqj(dofs); @@ -88,10 +86,10 @@ bool setStaticState(std::shared_ptr kinDyn) } return kinDyn->setRobotState(transform, - iDynTree::make_span(qj.data(), qj.size()), - iDynTree::make_span(baseVel.data(), baseVel.size()), - iDynTree::make_span(dqj.data(), dqj.size()), - iDynTree::make_span(gravity.data(), gravity.size())); + qj, + iDynTree::make_span(baseVel.data(), manif::SE3d::Tangent::DoF), + dqj, + gravity); } IParametersHandler::shared_ptr createModelParameterHandler() @@ -198,7 +196,7 @@ TEST_CASE("KinDynWrapper Test") gravity); // Forward dynamics - Eigen::VectorXd jointAccFD = Eigen::VectorXd(numJoints); + Eigen::VectorXd jointAccFD(numJoints); REQUIRE(kinDynWrapperList[0]->forwardDynamics(jointTrq, Eigen::VectorXd::Zero(numJoints), Eigen::VectorXd::Zero(numJoints), @@ -219,7 +217,7 @@ TEST_CASE("KinDynWrapper Test") tempVec += massMatrix.topRightCorner(6, numJoints) * jointAcc; trqExt.head(6) = tempVec + genTrq.head(6); - Eigen::VectorXd nuDot = Eigen::VectorXd(6 + numJoints); + Eigen::VectorXd nuDot(6 + numJoints); REQUIRE(kinDynWrapperList[0]->forwardDynamics(jointTrq, Eigen::VectorXd::Zero(numJoints), trqExt, diff --git a/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityStateDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityStateDynamicsTest.cpp index 2488166496..bf29d73ddc 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityStateDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityStateDynamicsTest.cpp @@ -8,12 +8,8 @@ #include #include -#include -#include #include -#include -#include #include #include @@ -59,8 +55,7 @@ TEST_CASE("Zero Velocity Dynamics") currentState(index) = GENERATE(take(1, random(-100, 100))); } - Eigen::VectorXd state; - state.resize(sizeVariable * variableHandler.getNumberOfVariables()); + Eigen::VectorXd state(sizeVariable * variableHandler.getNumberOfVariables()); state.setZero(); state.segment(variableHandler.getVariable("tau_m").offset, variableHandler.getVariable("tau_m").size) = currentState;