From ad5673c837f7a69ba947e5c142f2f4db225e418c Mon Sep 17 00:00:00 2001 From: Ines Date: Tue, 31 Oct 2023 17:42:39 +0100 Subject: [PATCH] Add KinDynWrapper test --- src/Estimators/CMakeLists.txt | 4 +- .../RobotDynamicsEstimator.h | 172 ------ .../RobotDynamicsEstimator/UkfMeasurement.h | 249 -------- .../RobotDynamicsEstimator/UkfModel.h | 112 ---- .../RobotDynamicsEstimator/UkfState.h | 192 ------ .../src/AccelerometerMeasurementDynamics.cpp | 2 - src/Estimators/src/KinDynWrapper.cpp | 2 +- src/Estimators/src/RobotDynamicsEstimator.cpp | 555 ------------------ src/Estimators/src/UkfMeasurement.cpp | 460 --------------- src/Estimators/src/UkfModel.cpp | 198 ------- src/Estimators/src/UkfState.cpp | 351 ----------- .../RobotDynamicsEstimator/CMakeLists.txt | 7 + .../GyroscopeMeasurementDynamicsTest.cpp | 7 +- .../KinDynWrapperTest.cpp | 230 ++++++++ 14 files changed, 243 insertions(+), 2298 deletions(-) delete mode 100644 src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/RobotDynamicsEstimator.h delete mode 100644 src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfMeasurement.h delete mode 100644 src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfModel.h delete mode 100644 src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h delete mode 100644 src/Estimators/src/RobotDynamicsEstimator.cpp delete mode 100644 src/Estimators/src/UkfMeasurement.cpp delete mode 100644 src/Estimators/src/UkfModel.cpp delete mode 100644 src/Estimators/src/UkfState.cpp create mode 100644 src/Estimators/tests/RobotDynamicsEstimator/KinDynWrapperTest.cpp diff --git a/src/Estimators/CMakeLists.txt b/src/Estimators/CMakeLists.txt index 1208bb146b..74820fdbe3 100644 --- a/src/Estimators/CMakeLists.txt +++ b/src/Estimators/CMakeLists.txt @@ -29,13 +29,11 @@ if(FRAMEWORK_COMPILE_RobotDynamicsEstimator) NAME RobotDynamicsEstimator SOURCES src/SubModel.cpp src/KinDynWrapper.cpp src/Dynamics.cpp src/ZeroVelocityStateDynamics.cpp src/JointVelocityStateDynamics.cpp src/ConstantMeasurementModel.cpp src/AccelerometerMeasurementDynamics.cpp src/GyroscopeMeasurementDynamics.cpp - src/FrictionTorqueStateDynamics.cpp src/MotorCurrentMeasurementDynamics.cpp src/UkfModel.cpp - src/UkfState.cpp src/UkfMeasurement.cpp src/RobotDynamicsEstimator.cpp + src/FrictionTorqueStateDynamics.cpp src/MotorCurrentMeasurementDynamics.cpp SUBDIRECTORIES tests/RobotDynamicsEstimator PUBLIC_HEADERS ${H_PREFIX}/SubModel.h ${H_PREFIX}/KinDynWrapper.h ${H_PREFIX}/Dynamics.h ${H_PREFIX}/ZeroVelocityStateDynamics.h ${H_PREFIX}/JointVelocityStateDynamics.h ${H_PREFIX}/ConstantMeasurementModel.h ${H_PREFIX}/AccelerometerMeasurementDynamics.h ${H_PREFIX}/GyroscopeMeasurementDynamics.h ${H_PREFIX}/FrictionTorqueStateDynamics.h ${H_PREFIX}/MotorCurrentMeasurementDynamics.h - ${H_PREFIX}/UkfModel.h ${H_PREFIX}/UkfState.h ${H_PREFIX}/UkfMeasurement.h ${H_PREFIX}/RobotDynamicsEstimator.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 diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/RobotDynamicsEstimator.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/RobotDynamicsEstimator.h deleted file mode 100644 index ef905d4b34..0000000000 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/RobotDynamicsEstimator.h +++ /dev/null @@ -1,172 +0,0 @@ -/** - * @file RobotDynamicsEstimator.h - * @authors Ines Sorrentino - * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and - * distributed under the terms of the BSD-3-Clause license. - */ - -#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_ROBOT_DYNAMICS_ESTIMATOR_H -#define BIPEDAL_LOCOMOTION_ESTIMATORS_ROBOT_DYNAMICS_ESTIMATOR_H - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -namespace BipedalLocomotion -{ -namespace Estimators -{ -namespace RobotDynamicsEstimator -{ - -/** - * RobotDynamicsEstimatorInput of the RobotDynamicsEstimator containing both the inputs and the - * measurements for the Unscented Kalman Filter - */ -struct RobotDynamicsEstimatorInput -{ - manif::SE3d basePose; /**< Pose describing the robot base position and orientation. */ - manif::SE3d::Tangent baseVelocity; /**< Velocity of the robot base. */ - manif::SE3d::Tangent baseAcceleration; /**< Mixed acceleration of the robot base. */ - Eigen::VectorXd jointPositions; /**< Joints positions in rad. */ - Eigen::VectorXd jointVelocities; /**< Joints velocities in rad per sec. */ - Eigen::VectorXd motorCurrents; /**< Motor currents in Ampere. */ - std::map ftWrenches; /**< Wrenches measured by force/torque sensors. */ - std::map linearAccelerations; /**< Linear accelerations measured by accelerometer sensors. */ - std::map angularVelocities; /**< Angular velocities measured by gyroscope sensors. */ - Eigen::VectorXd friction; /**< Friction torques in Nm. */ -}; - -/** - * RobotDynamicsEstimatorOutput of the RobotDynamicsEstimator which represents the estimation of the - * Unscented Kalman Filter - */ -struct RobotDynamicsEstimatorOutput -{ - Eigen::VectorXd ds; /**< Joints velocities in rad per sec. */ - Eigen::VectorXd tau_m; /**< Motor toruqes in Nm. */ - Eigen::VectorXd tau_F; /**< Motor friction torques in Nm. */ - std::map ftWrenches; /**< Wrenches at the force/torque sensors. */ - std::map ftWrenchesBiases; /**< Biases of the force/torque sensors. */ - std::map accelerometerBiases; /**< Biases of the accelerometer sensors. */ - std::map gyroscopeBiases; /**< Biases of the gyroscope sensors. */ - std::map contactWrenches; /**< External contact wrenches. */ -}; - -/** - * RobotDynamicsEstimator is a concrete class and implements a robot dynamics estimator. - * The estimator is here implemented as an Unscented Kalman Filter. The user can build the estimator - * by using the build method or can initialize the RobotDynamicsEstimator object and then create the - * UkfState model and UkfMeasurement model which are then passed to the `bfl::UKFPrediction` and - * `bfl::UKFCorrection` objects respectively. To run an estimation step the user should set the - * input using `setInput`, call the `advance` method to perform an estimation step, use `getOutput` - * to get the estimation result. - */ -class RobotDynamicsEstimator : public System::Advanceable -{ - /** - * Private implementation - */ - struct Impl; - std::unique_ptr m_pimpl; - -public: - /** - * Constructor. - */ - RobotDynamicsEstimator(); - - /** - * Destructor. - */ - virtual ~RobotDynamicsEstimator(); - - /** - * Initialize the RobotDynamicsEstimator. - * @param handler pointer to the IParametersHandler - * @note - * | Group | Parameter Name | Type | Description | Mandatory | - * |:---------:|:------------------------------:|:---------------:|:----------------------------------------------------------------------------------------------:|:---------:| - * | `GENERAL` | `sampling_time` | `double` | Sampling time | Yes | - * | `UKF` | `alpha` | `double` | `alpha` parameter of the unscented kalman filter | Yes | - * | `UKF` | `beta` | `double` | `beta` parameter of the unscented kalman filter | Yes | - * | `UKF` | `kappa` | `double` | `kappa` parameter of the unscented kalman filter | Yes | - * @return True in case of success, false otherwise. - */ - bool initialize(std::weak_ptr handler) override; - - /** - * @brief Finalize the estimator. - * @param stateVariableHandler reference to a `System::VariablesHandler` object describing the ukf state. - * @param measurementVariableHandler reference to a `System::VariablesHandler` object describing the ukf measurement. - * @param kinDynFullModel a pointer to an iDynTree::KinDynComputations object. - * @note You should call this method after initialized the estimator and created the UkfState model - * which builds the handler needed in input to this method. - * @return true in case of success, false otherwise. - */ - bool finalize(const System::VariablesHandler& stateVariableHandler, - const System::VariablesHandler& measurementVariableHandler, - std::shared_ptr kinDynFullModel); - - /** - * @brief build a robot dynamics estimator implementing an unscented kalman filter. - * @param handler pointer to the IParametersHandler interface. - * @param kinDynFullModel a pointer to an iDynTree::KinDynComputations object that will be shared among - * all the dynamics. - * @param subModelList a list of SubModel objects - * @param kinDynWrapperList a list of pointers to a `KinDynWrapper` objects that will be shared among - * all the dynamics - * @return a RobotDynamicsEstimator. In case of issues an invalid RobotDynamicsEstimator - * will be returned. - */ - static std::unique_ptr - build(std::weak_ptr handler, - std::shared_ptr kinDynFullModel, - const std::vector& subModelList, - const std::vector>& kinDynWrapperList); - - /** - * @brief set the initial state of the estimator. - * @param initialState a reference to an `Output` object. - * @return true in case of success, false otherwise. - */ - bool setInitialState(const RobotDynamicsEstimatorOutput& initialState); - - /** - * @brief Determines the validity of the object retrieved with getOutput() - * @return True if the object is valid, false otherwise. - */ - bool isOutputValid() const override; - - /** - * @brief Advance the internal state. This may change the value retrievable from getOutput(). - * @return True if the advance is successfull. - */ - bool advance() override; - - /** - * Set the input for the estimator. - * @param input is a struct containing the input of the estimator. - */ - bool setInput(const RobotDynamicsEstimatorInput& input) override; - - /** - * Get the output of the ukf - * @return A struct containing the ukf estimation result. - */ - const RobotDynamicsEstimatorOutput& getOutput() const override; - -}; // class RobotDynamicsEstimator - -} // namespace RobotDynamicsEstimator -} // namespace Estimators -} // namespace BipedalLocomotion - -#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_ROBOT_DYNAMICS_ESTIMATOR_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfMeasurement.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfMeasurement.h deleted file mode 100644 index 531c219955..0000000000 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfMeasurement.h +++ /dev/null @@ -1,249 +0,0 @@ -/** - * @file UkfMeasurement.h - * @authors Ines Sorrentino - * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and - * distributed under the terms of the BSD-3-Clause license. - */ - -#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_UKF_MEASUREMENT_H -#define BIPEDAL_LOCOMOTION_ESTIMATORS_UKF_MEASUREMENT_H - -#include -#include -#include - -// BayesFilters -#include - -// BLF -#include -#include -#include -#include -#include -#include - -namespace BipedalLocomotion -{ -namespace Estimators -{ -namespace RobotDynamicsEstimator -{ - -/** - * UkfMeasurement is a concrete class that represents the Measurement of the estimator. - * The user should build the dynamic model of the measurement, setting a variable handler - * describing the variables composing the measurement vector, the list of the dynamic models - * associated to each variable, and the matrix of covariances associated to the variable in the - * measurement vector. The user should set also a ukf input provider - * which provides the inputs needed to update the ukf measurement dynamics. - */ - -class UkfMeasurement : public bfl::AdditiveMeasurementModel, UkfModel -{ - /** - * Private implementation - */ -// struct Impl; -// std::unique_ptr m_pimpl; - bfl::VectorDescription m_measurementDescription; - bfl::VectorDescription m_inputDescription; - Eigen::MatrixXd m_covarianceR; /**< Covariance matrix. */ - int m_measurementSize{0}; /**< Length of the measurement vector. */ - System::VariablesHandler m_measurementVariableHandler; /**< Variable handler describing the - measurement vector. */ - Eigen::VectorXd m_tempPredictedMeas; - Eigen::MatrixXd m_predictedMeasurement; /**< Vector containing the updated measurement. */ - -public: -// /** -// * Constructor. -// */ -// UkfMeasurement(); - -// /** -// * Destructor. -// */ -// virtual ~UkfMeasurement(); - - /** - * Build the ukf measurement model - * @param kinDyn a pointer to an iDynTree::KinDynComputations object that will be shared among - * all the dynamics. - * @param subModelList a vector of SubModel objects. - * @param kinDynWrapperList a vector of pointers to KinDynWrapper objects - * @param handler pointer to the IParametersHandler interface. - * @param stateVariableHandler a variable handler describing the variables in the state vector of the ukf. - * @note the following parameters are required by the class - * | Parameter Name | Type | Description | Mandatory | - * |:------------------------------:|:---------------:|:----------------------------------------------------------------------------------------------:|:---------:| - * | `sampling_time` | `double` | Sampling time. | Yes | - * | `dynamics_list` |`vector` | List of dynamics composing the measurement model. | Yes | - * For **each** dynamics listed in the parameter `dynamics_list` the user must specified all the parameters - * required by the dynamics itself but `dT` since is already specified in the parent group. - * Moreover the following parameters are required for each dynamics. - * | Group | Parameter Name | Type | Description | Mandatory | - * |:-------------:|:------------------------------:|:--------------------:|:----------------------------------------------------------------------------------------------:|:---------:| - * |`DYNAMICS_NAME`| `name` | `string` | String representing the name of the dynamics. | Yes | - * |`DYNAMICS_NAME`| `elements` |`std::vector` | Vector of strings representing the elements composing the specific dynamics. | No | - * |`DYNAMICS_NAME`| `covariance` |`std::vector` | Vector of double containing the covariance associated to each element. | Yes | - * |`DYNAMICS_NAME`| `dynamic_model` | `string` | String representing the type of dynamics. The string should match the name of the C++ class. | Yes | - * |`DYNAMICS_NAME`| `use_bias` | `boolean` | Boolean saying if an additive bias must be used in the dynamic model. | No | - * |`DYNAMICS_NAME`| `use_bias` | `boolean` | Boolean saying if an additive bias must be used in the dynamic model. | No | - * `DYNAMICS_NAME` is a placeholder for the name of the dynamics contained in the `dynamics_list` list. - * `name` can contain only the following values ("ds", "i_m", "*_ft_sensor", "*_ft_acc", "*_ft_gyro"). - * @note The following `ini` file presents an example of the configuration that can be used to - * build the UkfMeasurement. - * - * UkfMeasurement.ini - * - * dynamics_list ("JOINT_VELOCITY", "MOTOR_CURRENT", "RIGHT_LEG_FT", "RIGHT_FOOT_REAR_GYRO") - * - * [JOINT_VELOCITY] - * name "ds" - * elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") - * covariance (1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3) - * dynamic_model "JointVelocityDynamics" - * - * [FRICTION_TORQUE] - * name "tau_F" - * elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") - * covariance (1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-1) - * dynamic_model "FrictionTorqueDynamics" - * friction_k0 (9.106, 5.03, 4.93, 12.88, 14.34, 1.12) - * friction_k1 (200.0, 6.9, 200.0, 59.87, 200.0, 200.0) - * friction_k2 (1.767, 5.64, 0.27, 2.0, 3.0, 0.0) - * - * [RIGHT_LEG_FT] - * name "r_leg_ft_sensor" - * elements ("fx", "fy", "fz", "mx", "my", "mz") - * covariance (1e-3, 1e-3, 1e-3, 1e-4, 1e-4, 1e-4) - * dynamic_model "ConstantMeasurementModel" - * - * [RIGHT_FOOT_REAR_GYRO_BIAS] - * name "r_foot_rear_ft_gyro_bias" - * elements ("x", "y", "z") - * covariance (8.2e-8, 1e-2, 9.3e-3) - * dynamic_model "ConstantMeasurementModel" - * - * ~~~~~ - * @return a std::unique_ptr to the UkfMeasurement. - * In case of issues, an empty BipedalLocomotion::System::VariablesHandler - * and an invalid pointer will be returned. - */ - static std::unique_ptr build(std::weak_ptr handler, - System::VariablesHandler& stateVariableHandler, - std::shared_ptr kinDynFullModel, - const std::vector& subModelList, - const std::vector>& kinDynWrapperList); - - /** - * Initialize the ukf measurement model. - * @param handler pointer to the IParametersHandler interface. - * @note the following parameters are required by the class - * | Parameter Name | Type | Description | Mandatory | - * |:------------------------------:|:---------------:|:----------------------------------------------------------------------------------------------:|:---------:| - * | `sampling_time` | `double` | Sampling time. | Yes | - * @return True in case of success, false otherwise. - */ - bool initialize(std::weak_ptr handler); - - /** - * Finalize the UkfMeasurement. - * @param handler variable handler. - * @note You should call this method after you initialize the UkfMeasurement. - * @return true in case of success, false otherwise. - */ - bool finalize(const System::VariablesHandler& handler); - - /** - * @brief setUkfInputProvider set the provider for the ukf input. - * @param ukfInputProvider a pointer to a structure containing the joint positions and the robot base pose, velocity and acceleration. - */ - void setUkfInputProvider(std::shared_ptr ukfInputProvider); - - /** - * @brief getMeasurementVariableHandler access the `System::VariablesHandler` instance created during the initialization phase. - * @return the measurement variable handler containing all the measurement variables and their sizes and offsets. - */ - System::VariablesHandler& getMeasurementVariableHandler(); - - /** - * @brief predictedMeasure predict the new measurement depending on the state computed by the predict step. - * @param cur_states is the state computed by the prediction phase. - * @return a std::pair where the bool value says if the measurement - * prediciton is done correctly and the bfl::Data is the predicted measure. - */ - std::pair predictedMeasure(const Eigen::Ref& cur_states) const override; - - /** - * @brief getNoiseCovarianceMatrix access the `Eigen::MatrixXd` representing the process covariance matrix. - * @return a boolean value and the measurement noise covariance matrix. - */ - std::pair getNoiseCovarianceMatrix() const override; - - /** - * @brief setProperty is not implemented. - * @param property is a string. - * @return false as it is not implemented. - */ - bool setProperty(const std::string& property) override { return false; }; - - /** - * @brief getMeasurementDescription access the `bfl::VectorDescription`. - * @return the measurement vector description. - */ - bfl::VectorDescription getMeasurementDescription() const override; - - /** - * @brief getInputDescription access the `bfl::VectorDescription`. - * @return the input vector description. - */ - bfl::VectorDescription getInputDescription() const override; - - /** - * @brief getMeasurementSize access the length of the measurement vector - * @return the length of measurement vector - */ - std::size_t getMeasurementSize(); - - /** - * Set a `System::VariableHandler` describing the variables composing the state. - * @param stateVariableHandler is the variable handler - */ - void setStateVariableHandler(System::VariablesHandler stateVariableHandler); - - /** - * @brief innovation computes the innovation step of the ukf update as the difference between the predicted_measurement - * and the measurement. - * @param predicted_measurements is a `blf::Data` reference representing the measurement predicted in the update step. - * @param measurements is a `blf::Data` reference representing the measurements coming from the user. - * @return a `std::pair` where the boolean value is always true and the `bfl::Data` is the innovation term. - */ - std::pair innovation(const bfl::Data& predicted_measurements, const bfl::Data& measurements) const override; - - /** - * @brief measure get the updated measurement. - * @param data is a `const blf::Data` reference and is optional parameter. - * @return a pair of a boolean value which is always true and the measurements. - */ - std::pair measure(const bfl::Data& data = bfl::Data()) const override; - - /** - * @brief freeze update the measurement using data from sensors. - * @param data is a generic object representing data coming from sensors. - * @return true. - * @note data in this case must be a `std::map` where - * the first element represents the name of each measurement dynamics - * and the second element is the vector containing the measurement - * of the sensor associated to that variable. - */ - bool freeze(const bfl::Data& data = bfl::Data()) override; - -}; // classUkfMeasurement - -} // namespace RobotDynamicsEstimator -} // namespace Estimators -} // namespace BipedalLocomotion - -#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_UKF_MEASUREMENT_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfModel.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfModel.h deleted file mode 100644 index 9604ad43d0..0000000000 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfModel.h +++ /dev/null @@ -1,112 +0,0 @@ -/** - * @file UkfModel.h - * @authors Ines Sorrentino - * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and - * distributed under the terms of the BSD-3-Clause license. - */ - -#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_UKF_MODEL_H -#define BIPEDAL_LOCOMOTION_ESTIMATORS_UKF_MODEL_H - -#include -#include -#include - -// BLF -#include -#include -#include -#include -#include - -namespace BipedalLocomotion -{ -namespace Estimators -{ -namespace RobotDynamicsEstimator -{ - -/** - * UkfModel is a base class to define the process and measurement model of the UKF. - * It includes all the methods shared between the two concrete classes, as the - * update of the robot dynamics and the robot state. - */ -class UkfModel -{ -protected: - bool m_isInitialized{false}; - bool m_isFinalized{false}; - Eigen::Vector3d m_gravity{0, 0, -Math::StandardAccelerationOfGravitation}; /**< Gravity vector. */ - double m_dT; /**< Sampling time */ - std::vector>> m_dynamicsList; /**< List of the - dynamics - composing the - model. - */ - System::VariablesHandler m_stateVariableHandler; /**< Variable handler describing the state - vector. */ - std::shared_ptr m_kinDynFullModel; /**< KinDynComputation object for - the full model. */ - std::vector m_subModelList; /**< List of SubModel object describing the sub-models - composing the full model. */ - std::vector> m_kinDynWrapperList; /**< List of - KinDynWrapper - objects containing - kinematic and dynamic - information specific - of each sub-model. */ - std::shared_ptr m_ukfInputProvider; /**< Provider containing the updated - robot state. */ - UKFInput m_ukfInput; /**< Struct containing the inputs for the ukf populated by the - ukfInputProvider. */ - Eigen::VectorXd m_jointVelocityState; /**< Joint velocity computed by the ukf. */ - Eigen::VectorXd m_jointAccelerationState; /**< Joint acceleration computed from forward dynamics - which depends on the current ukf state. */ - Eigen::VectorXd m_currentState; /**< State estimated in the previous step. */ - std::vector m_subModelJointPos; /**< List of sub-model joint velocities. */ - std::vector m_subModelJointVel; /**< List of sub-model joint velocities. */ - std::vector m_subModelJointAcc; /**< List of sub-model joint accelerations. */ - std::vector m_subModelNuDot; /**< List of sub-model accelerations (base + joints - = nudot). */ - std::vector m_subModelJointMotorTorque; /**< List of sub-model joint motor - torques. */ - std::vector m_subModelFrictionTorque; /**< List of sub-model friction torques. */ - std::map m_FTMap; /**< The map contains names of the ft sensors and - values of the wrench. */ - std::map m_accBiasMap; /**< The map contains names of the accelerometer sensor bias and - values of the bias. */ - std::map m_gyroBiasMap; /**< The map contains names of the gyroscope sensor bias and - values of the bias. */ - std::map m_extContactMap; /**< The map contains names of the ft - sensors and values of the wrench. */ - std::vector m_totalTorqueFromContacts; /**< Joint torques due to known and - unknown contacts on the sub-model. */ - Math::Wrenchd m_wrench; /**< Joint torques due to a specific contact. */ - Eigen::VectorXd m_measurement; /**< Measurements coming from the sensors. */ - std::map m_measurementMap; /**< Measurement map . */ - manif::SE3d::Tangent m_subModelBaseVelTemp; /**< Velocity of the base of the sub-model. */ - std::vector m_tempJacobianList; /**< List of jacobians per eache submodel. */ - manif::SE3d m_worldTBase; /**< Sub-model base pose wrt the inertial frame */ - int m_offsetMeasurement; /**< Offset used to fill the measurement vector. */ -// Eigen::VectorXd m_prediction; /**< Vector containing the predicted state or measurement. */ - -public: - /** - * @brief unpackState splits the state vector in all the variables composing the state. - */ - void unpackState(); - - /** - * @brief updateState updates the robot state and dynamics and of the sub-model states and - * dynamics by using the estimation of the previous step. - * @return true in case of success, false otherwise. - */ - bool updateState(); -}; - -} // namespace RobotDynamicsEstimator -} // namespace Estimators -} // namespace BipedalLocomotion - -#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_UKF_MODEL_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h deleted file mode 100644 index 57fa48af7b..0000000000 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h +++ /dev/null @@ -1,192 +0,0 @@ -/** - * @file UkfState.h - * @authors Ines Sorrentino - * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and - * distributed under the terms of the BSD-3-Clause license. - */ - -#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_UKF_STATE_H -#define BIPEDAL_LOCOMOTION_ESTIMATORS_UKF_STATE_H - -#include -#include -#include - -// BayesFilters -#include - -// BLF -#include -#include -#include -#include -#include - -namespace BipedalLocomotion -{ -namespace Estimators -{ -namespace RobotDynamicsEstimator -{ - -/** - * UkfState is a concrete class that represents the State of the estimator. - * The user should build the dynamic model of the state setting a variable handler - * describing the variables composing the state, the list of the dynamic model associated - * to each variable in the variable handler, and the matrix of covariances - * associated to the state. The user should set also a ukf input provider - * which provides the inputs needed to update the ukf process dynamics. - */ -class UkfState : public bfl::AdditiveStateModel, UkfModel -{ - Eigen::MatrixXd m_covarianceQ; /**< Covariance matrix. */ - Eigen::MatrixXd m_initialCovariance; /**< Initial covariance matrix. */ - std::size_t m_stateSize; /**< Length of the state vector. */ - Eigen::VectorXd m_nextState; /**< Vector containing all the updated states. */ - -public: - /** - * Build the ukf state model - * @param kinDyn a pointer to an iDynTree::KinDynComputations object that will be shared among - * all the dynamics. - * @param subModelList a vector of pairs (SubModel, KinDynWrapper) that will be shared among all the dynamics. - * @param handler pointer to the IParametersHandler interface. - * @note the following parameters are required by the class - * | Parameter Name | Type | Description | Mandatory | - * |:------------------------------:|:---------------:|:----------------------------------------------------------------------------------------------:|:---------:| - * | `dT` | `double` | Sampling time. | Yes | - * | `dynamics_list` |`vector` | List of dynamics composing the state model. | Yes | - * For **each** dynamics listed in the parameter `dynamics_list` the user must specified all the parameters - * required by the dynamics itself but `dT` since is already specified in the parent group. - * Moreover the following parameters are required for each dynamics. - * | Group | Parameter Name | Type | Description | Mandatory | - * |:-------------:|:------------------------------:|:--------------------:|:----------------------------------------------------------------------------------------------:|:---------:| - * |`DYNAMICS_NAME`| `name` | `string` | String representing the name of the dynamics. | Yes | - * |`DYNAMICS_NAME`| `elements` |`std::vector` | Vector of strings representing the elements composing the specific dynamics. | No | - * |`DYNAMICS_NAME`| `covariance` |`std::vector` | Vector of double containing the covariance associated to each element. | Yes | - * |`DYNAMICS_NAME`| `dynamic_model` | `string` | String representing the type of dynamics. The string should match the name of the C++ class. | Yes | - * |`DYNAMICS_NAME`| `friction_k0` |`std::vector` | Vector of double containing the coefficient k0 of the friction model of each element. | No | - * |`DYNAMICS_NAME`| `friction_k1` |`std::vector` | Vector of double containing the coefficient k1 of the friction model of each element. | No | - * |`DYNAMICS_NAME`| `friction_k2` |`std::vector` | Vector of double containing the coefficient k2 of the friction model of each element. | No | - * `DYNAMICS_NAME` is a placeholder for the name of the dynamics contained in the `dynamics_list` list. - * `name` can contain only the following values ("ds", "tau_m", "tau_F", "*_ft_sensor", "*_ft_sensor_bias", "*_ft_acc_bias", "*_ftgyro_bias"). - * @note The following `ini` file presents an example of the configuration that can be used to - * build the UkfState. - * - * UkfState.ini - * - * dynamics_list ("JOINT_VELOCITY", "FRICTION_TORQUE", "RIGHT_LEG_FT", "RIGHT_FOOT_REAR_GYRO_BIAS") - * - * [JOINT_VELOCITY] - * name "ds" - * elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") - * covariance (1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3) - * dynamic_model "JointVelocityDynamics" - * - * [FRICTION_TORQUE] - * name "tau_F" - * elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") - * covariance (1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-1) - * dynamic_model "FrictionTorqueDynamics" - * friction_k0 (9.106, 5.03, 4.93, 12.88, 14.34, 1.12) - * friction_k1 (200.0, 6.9, 200.0, 59.87, 200.0, 200.0) - * friction_k2 (1.767, 5.64, 0.27, 2.0, 3.0, 0.0) - * - * [RIGHT_LEG_FT] - * name "r_leg_ft_sensor" - * elements ("fx", "fy", "fz", "mx", "my", "mz") - * covariance (1e-3, 1e-3, 1e-3, 1e-4, 1e-4, 1e-4) - * dynamic_model "ZeroVelocityStateDynamics" - * - * [RIGHT_FOOT_REAR_GYRO_BIAS] - * name "r_foot_rear_ft_gyro_bias" - * elements ("x", "y", "z") - * covariance (8.2e-8, 1e-2, 9.3e-3) - * dynamic_model "ZeroVelocityStateDynamics" - * - * ~~~~~ - * @return a std::unique_ptr to the UkfState. - * In case of issues, an empty BipedalLocomotion::System::VariablesHandler - * and an invalid pointer will be returned. - */ - static std::unique_ptr build(std::weak_ptr handler, - std::shared_ptr kinDynFullModel, - const std::vector& subModelList, - const std::vector>& kinDynWrapperList); - - /** - * Initialize the ukf state model. - * @param handler pointer to the IParametersHandler interface. - * @note the following parameters are required by the class - * | Parameter Name | Type | Description | Mandatory | - * |:------------------------------:|:---------------:|:----------------------------------------------------------------------------------------------:|:---------:| - * | `dT` | `double` | Sampling time. | Yes | - * @return True in case of success, false otherwise. - */ - bool initialize(std::weak_ptr handler); - - /** - * Finalize the UkfState. - * @param handler variable handler. - * @note You should call this method after you initialize the UkfState. - * @return true in case of success, false otherwise. - */ - bool finalize(const System::VariablesHandler& handler); - - /** - * @brief setUkfInputProvider set the provider for the ukf input. - * @param ukfInputProvider a pointer to a structure containing the joint positions and the robot base pose, velocity and acceleration. - */ - void setUkfInputProvider(std::shared_ptr ukfInputProvider); - - /** - * @brief getStateVariableHandler access the `System::VariablesHandler` instance created during the initialization phase. - * @return the state variable handler containing all the state variables and their sizes and offsets. - */ - System::VariablesHandler& getStateVariableHandler(); - - /** - * @brief propagate implements the prediction phase of the ukf - * @param cur_states is the state computed at the previous step - * @param prop_states is the predicted state - */ - void propagate(const Eigen::Ref& cur_states, Eigen::Ref prop_states) override; - - /** - * @brief getNoiseCovarianceMatrix access the `Eigen::MatrixXd` representing the process covariance matrix. - * @return the process noise covariance matrix. - */ - Eigen::MatrixXd getNoiseCovarianceMatrix() override; - - /** - * @brief setProperty is not implemented. - * @param property is a string. - * @return false as it is not implemented. - */ - bool setProperty(const std::string& property) override { return false; }; - - /** - * @brief getStateDescription access the `bfl::VectorDescription`. - * @return the state vector description. - */ - bfl::VectorDescription getStateDescription() override; - - /** - * @brief getStateSize access the length of the state vector. - * @return the length of state vector. - */ - std::size_t getStateSize(); - - /** - * @brief getInitialStateCovarianceMatrix access the `Eigen::MatrixXd` representing the initial state covariance matrix. - * @return a Eigen reference to the Eigen Matrix covariance. - */ - Eigen::Ref getInitialStateCovarianceMatrix(); - -}; // class UKFModel - -} // namespace RobotDynamicsEstimator -} // namespace Estimators -} // namespace BipedalLocomotion - -#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_UKF_STATE_H diff --git a/src/Estimators/src/AccelerometerMeasurementDynamics.cpp b/src/Estimators/src/AccelerometerMeasurementDynamics.cpp index 5e50bfa9c1..6c59dbb8f2 100644 --- a/src/Estimators/src/AccelerometerMeasurementDynamics.cpp +++ b/src/Estimators/src/AccelerometerMeasurementDynamics.cpp @@ -11,7 +11,6 @@ #include #include #include -#include namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; @@ -185,7 +184,6 @@ bool RDE::AccelerometerMeasurementDynamics::update() } } - for (int index = 0; index < m_subModelsWithAccelerometer.size(); index++) { m_subModelBaseAcceleration diff --git a/src/Estimators/src/KinDynWrapper.cpp b/src/Estimators/src/KinDynWrapper.cpp index 10ba655ca6..f6c36ef4e9 100644 --- a/src/Estimators/src/KinDynWrapper.cpp +++ b/src/Estimators/src/KinDynWrapper.cpp @@ -83,7 +83,7 @@ bool KinDynWrapper::forwardDynamics(Eigen::Ref motorTorqu return false; } - m_FTvBaseDot = m_massMatrix.block(6, 0, this->getNrOfDegreesOfFreedom(), 6) * baseAcceleration.coeffs(); + m_FTvBaseDot = m_massMatrix.bottomLeftCorner(this->getNrOfDegreesOfFreedom(), 6) * baseAcceleration.coeffs(); m_totalJointTorques = motorTorqueAfterGearbox - frictionTorques + tauExt - m_generalizedBiasForces.tail(this->getNrOfDegreesOfFreedom()) diff --git a/src/Estimators/src/RobotDynamicsEstimator.cpp b/src/Estimators/src/RobotDynamicsEstimator.cpp deleted file mode 100644 index 6109c3566d..0000000000 --- a/src/Estimators/src/RobotDynamicsEstimator.cpp +++ /dev/null @@ -1,555 +0,0 @@ -/** - * @file RobotDynamicsEstimator.cpp - * @authors Ines Sorrentino - * @copyright 2023 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 -#include -#include -#include -#include -#include -#include - -using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; -using namespace BipedalLocomotion; - -struct RobotDynamicsEstimator::Impl -{ - RobotDynamicsEstimatorOutput estimatorOutput; /**< Output of the estimator. */ - - UKFInput ukfInput; /**< Object to set the provider. */ - std::shared_ptr inputProvider; /**< Shared pointer used by all the dynamics. It needs to be updated here. */ - - std::map ukfMeasurementFromSensors; /**< This map containes the measurement coming from the sensors - and needed for the correction phase of the estimation. */ - - bfl::Gaussian predictedState; /**< Predicted state computed by the `predict` method. */ - bfl::Gaussian correctedState; /**< Corrected state computed by the `correct` method. */ - - double dT; /**< Sampling time. */ - double alpha, beta, kappa; /**< Parameters of the unscented kalman filter. */ - - std::unique_ptr stateModel; /**< UKF state model. */ - std::unique_ptr measurementModel; /**< UKF measurement model. */ - - std::unique_ptr ukfPrediction; - std::unique_ptr ukfCorrection; - - System::VariablesHandler stateHandler; /**< Handler of the ukf state. */ - System::VariablesHandler measurementHandler; /**< Handler of the ukf measurement. */ - - Eigen::MatrixXd initialStateCovariance; - - bool isInitialized{false}; - bool isFinalized{false}; - - bool isValid{false}; - - bool isInitialStateSet{false}; -}; - -RobotDynamicsEstimator::RobotDynamicsEstimator() -{ - m_pimpl = std::make_unique(); -} - -RobotDynamicsEstimator::~RobotDynamicsEstimator() = default; - -bool RobotDynamicsEstimator::initialize(std::weak_ptr handler) -{ - constexpr auto logPrefix = "[RobotDynamicsEstimator::initialize]"; - - auto ptr = handler.lock(); - if (ptr == nullptr) - { - log()->error("{} The parameter handler is not valid.", logPrefix); - return false; - } - - auto groupGeneral = ptr->getGroup("GENERAL").lock(); - if (groupGeneral == nullptr) - { - log()->error("{} Error while retrieving the group GENERAL.", logPrefix); - return false; - } - - if (!groupGeneral->getParameter("sampling_time", m_pimpl->dT)) - { - log()->error("{} Error while retrieving the sampling time variable.", logPrefix); - return false; - } - - auto groupUkf = ptr->getGroup("UKF").lock(); - if (groupUkf == nullptr) - { - log()->error("{} Error while retrieving the group UKF.", logPrefix); - return false; - } - - if (!groupUkf->getParameter("alpha", m_pimpl->alpha)) - { - log()->error("{} Error while retrieving the alpha variable.", logPrefix); - return false; - } - - if (!groupUkf->getParameter("beta", m_pimpl->beta)) - { - log()->error("{} Error while retrieving the beta variable.", logPrefix); - return false; - } - - if (!groupUkf->getParameter("kappa", m_pimpl->kappa)) - { - log()->error("{} Error while retrieving the kappa variable.", logPrefix); - return false; - } - - m_pimpl->inputProvider = std::make_shared(); - - m_pimpl->isInitialized = true; - - return true; -} - -bool RobotDynamicsEstimator::finalize(const System::VariablesHandler& stateVariableHandler, - const System::VariablesHandler& measurementVariableHandler, - std::shared_ptr kinDynFullModel) -{ - // Resize variables for the estimation - m_pimpl->predictedState.resize(stateVariableHandler.getNumberOfVariables()); - m_pimpl->predictedState.mean().setZero(); - m_pimpl->predictedState.covariance() = m_pimpl->initialStateCovariance; - - m_pimpl->correctedState.resize(stateVariableHandler.getNumberOfVariables()); - m_pimpl->correctedState.mean().setZero(); - m_pimpl->correctedState.covariance() = m_pimpl->initialStateCovariance; - - // Set the provider initial state - m_pimpl->ukfInput.robotBasePose.setIdentity(); - m_pimpl->ukfInput.robotBaseVelocity.setZero(); - m_pimpl->ukfInput.robotBaseAcceleration.setZero(); - m_pimpl->ukfInput.robotJointPositions.resize(kinDynFullModel->model().getNrOfDOFs()); - m_pimpl->ukfInput.robotJointAccelerations.resize(kinDynFullModel->model().getNrOfDOFs()); - m_pimpl->inputProvider->setInput(m_pimpl->ukfInput); - - m_pimpl->isFinalized = true; - - return m_pimpl->isFinalized; -} - -std::unique_ptr RobotDynamicsEstimator::build(std::weak_ptr handler, - std::shared_ptr kinDynFullModel, - const std::vector& subModelList, - const std::vector>& kinDynWrapperList) -{ - constexpr auto logPrefix = "[RobotDynamicsEstimator::build]"; - - auto ptr = handler.lock(); - - if (ptr == nullptr) - { - log()->error("{} Invalid parameter handler.", logPrefix); - return nullptr; - } - - if (!kinDynFullModel->setFrameVelocityRepresentation(iDynTree::BODY_FIXED_REPRESENTATION)) - { - log()->error("{} Cannot set the frame velocity representation on the `iDynTree::KinDynComputation` " - "object describing the full robot model.", logPrefix); - return nullptr; - } - - std::unique_ptr estimator = std::make_unique(); - - if (!estimator->initialize(handler)) - { - log()->error("{} Error initializing the RobotDynamicsEstimator.", logPrefix); - return nullptr; - } - - auto groupUKF = ptr->getGroup("UKF").lock(); - if (groupUKF == nullptr) - { - log()->error("{} Error while retrieving the group UKF.", logPrefix); - return nullptr; - } - - auto groupUKFStateTmp = groupUKF->getGroup("UKF_STATE").lock(); - if (groupUKFStateTmp == nullptr) - { - log()->error("{} Error while retrieving the group UKF_STATE.", logPrefix); - return nullptr; - } - - auto groupUKFState = groupUKFStateTmp->clone(); - groupUKFState->setParameter("sampling_time", estimator->m_pimpl->dT); - - // Step 1 - // Create the state model for the ukf - estimator->m_pimpl->stateModel = UkfState::build(groupUKFState, - kinDynFullModel, - subModelList, - kinDynWrapperList); - if (estimator->m_pimpl->stateModel == nullptr) - { - log()->error("{} Error while creating the ukf state model.", logPrefix); - return nullptr; - } - - // Save a copy of the state variable handler - estimator->m_pimpl->stateHandler = estimator->m_pimpl->stateModel->getStateVariableHandler(); - - // Set the input provider - estimator->m_pimpl->stateModel->setUkfInputProvider(estimator->m_pimpl->inputProvider); - - // Get initial state covariance - estimator->m_pimpl->initialStateCovariance = estimator->m_pimpl->stateModel->getInitialStateCovarianceMatrix(); - - // Step 2 - // Initialize the unscented Kalman filter prediction step and pass the ownership of the state model - std::unique_ptr stateModelTemp = std::move(estimator->m_pimpl->stateModel); - estimator->m_pimpl->ukfPrediction = std::make_unique(std::move(stateModelTemp), - estimator->m_pimpl->alpha, - estimator->m_pimpl->beta, - estimator->m_pimpl->kappa); - - // Step 3 - // Create the measurement model for the ukf - auto groupUKFMeasTmp = groupUKF->getGroup("UKF_MEASUREMENT").lock(); - if (groupUKFMeasTmp == nullptr) - { - log()->error("{} Error while retrieving the group UKF_MEASUREMENT.", logPrefix); - return nullptr; - } - - auto groupUKFMeas = groupUKFMeasTmp->clone(); - groupUKFMeas->setParameter("sampling_time", estimator->m_pimpl->dT); - - // Create the measurement model for the ukf - estimator->m_pimpl->measurementModel = UkfMeasurement::build(groupUKFMeas, - estimator->m_pimpl->stateHandler, - kinDynFullModel, - subModelList, - kinDynWrapperList); - if (estimator->m_pimpl->measurementModel == nullptr) - { - log()->error("{} Error while creating the ukf measurement model.", logPrefix); - return nullptr; - } - - // Save a copy of the measurement variable handler - estimator->m_pimpl->measurementHandler = estimator->m_pimpl->measurementModel->getMeasurementVariableHandler(); - - // Set the input provider - estimator->m_pimpl->measurementModel->setUkfInputProvider(estimator->m_pimpl->inputProvider); - - // Step 4 - // Initialize the unscented Kalman filter correction step and pass the ownership of the measurement model. - std::unique_ptr measurementModelTemp = std::move(estimator->m_pimpl->measurementModel); - estimator->m_pimpl->ukfCorrection = std::make_unique(std::move(measurementModelTemp), - estimator->m_pimpl->alpha, - estimator->m_pimpl->beta, - estimator->m_pimpl->kappa); - - // Finalize the estimator - estimator->finalize(estimator->m_pimpl->stateHandler, estimator->m_pimpl->measurementHandler, kinDynFullModel); - - return estimator; -} - -bool RobotDynamicsEstimator::setInitialState(const RobotDynamicsEstimatorOutput& initialState) -{ - constexpr auto logPrefix = "[RobotDynamicsEstimator::setInitialState]"; - - System::VariablesHandler::VariableDescription variable; - - if (m_pimpl->stateHandler.getVariable("ds", variable)) - { - if (initialState.ds.size() != variable.size) - { - log()->error("{} Wrong size of variable `ds`. Found {}, expected {}.", logPrefix, initialState.ds.size(), variable.size); - return false; - } - m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = initialState.ds; - } - - if (m_pimpl->stateHandler.getVariable("tau_m", variable)) - { - if (initialState.tau_m.size() != variable.size) - { - log()->error("{} Wrong size of variable `tau_m`. Found {}, expected {}.", logPrefix, initialState.tau_m.size(), variable.size); - return false; - } - m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = initialState.tau_m; - } - - if (m_pimpl->stateHandler.getVariable("tau_F", variable)) - { - if (initialState.tau_F.size() != variable.size) - { - log()->error("{} Wrong size of variable `tau_F`. Found {}, expected {}.", logPrefix, initialState.tau_F.size(), variable.size); - return false; - } - m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = initialState.tau_F; - } - - for (auto const& [key, val] : initialState.ftWrenches) - { - if (m_pimpl->stateHandler.getVariable(key, variable)) - { - if (val.size() != variable.size) - { - log()->error("{} Wrong size of variable `{}`. Found {}, expected {}.", logPrefix, val, val.size(), variable.size); - return false; - } - m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = val; - } - } - - for (auto const& [key, val] : initialState.ftWrenchesBiases) - { - if (m_pimpl->stateHandler.getVariable(key, variable)) - { - if (val.size() != variable.size) - { - log()->error("{} Wrong size of variable `{}`. Found {}, expected {}.", logPrefix, val, val.size(), variable.size); - return false; - } - m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = val; - } - } - - for (auto const& [key, val] : initialState.gyroscopeBiases) - { - if (m_pimpl->stateHandler.getVariable(key, variable)) - { - if (val.size() != variable.size) - { - log()->error("{} Wrong size of variable `{}`. Found {}, expected {}.", logPrefix, val, val.size(), variable.size); - return false; - } - m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = val; - } - } - - for (auto const& [key, val] : initialState.accelerometerBiases) - { - if (m_pimpl->stateHandler.getVariable(key, variable)) - { - if (val.size() != variable.size) - { - log()->error("{} Wrong size of variable `{}`. Found {}, expected {}.", logPrefix, val, val.size(), variable.size); - return false; - } - m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = val; - } - } - - for (auto const& [key, val] : initialState.contactWrenches) - { -// log()->info("Contact wrench frame: {}", key); - if (m_pimpl->stateHandler.getVariable(key, variable)) - { - if (val.size() != variable.size) - { - log()->error("{} Wrong size of variable `{}`. Found {}, expected {}.", logPrefix, val, val.size(), variable.size); - return false; - } - m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = val; - } - } - - m_pimpl->estimatorOutput = initialState; - - m_pimpl->isInitialStateSet = true; - - return m_pimpl->isInitialStateSet; -} - -bool RobotDynamicsEstimator::isOutputValid() const -{ - return m_pimpl->isValid; -} - -bool RobotDynamicsEstimator::advance() -{ - constexpr auto logPrefix = "[RobotDynamicsEstimator::advance]"; - - // when advance is called the previous solution is no more valid - m_pimpl->isValid = false; - - if (!m_pimpl->isInitialized) - { - log()->error("{} Please call initialize() before advance().", logPrefix); - return false; - } - - if (!m_pimpl->isFinalized) - { - log()->error("{} Please call finalize() before advance().", logPrefix); - return false; - } - - if (!m_pimpl->isInitialStateSet) - { - log()->error("{} Please set the initial state before advance().", logPrefix); - return false; - } - - // Step 1 --> Predict - m_pimpl->ukfPrediction->predict(m_pimpl->correctedState, m_pimpl->predictedState); - -// log()->info("Covariance/n {}", m_pimpl->predictedState.covariance().block(m_pimpl->stateHandler.getVariable("r_upper_leg").offset, -// m_pimpl->stateHandler.getVariable("r_upper_leg").offset, -// m_pimpl->stateHandler.getVariable("r_upper_leg").size, -// m_pimpl->stateHandler.getVariable("r_upper_leg").size)); - -// log()->info("################# Predicted state ###################"); -// log()->info("ds\n{}", m_pimpl->predictedState.mean().segment(m_pimpl->stateHandler.getVariable("ds").offset, m_pimpl->stateHandler.getVariable("ds").size)); -// log()->info("tau m\n{}", m_pimpl->predictedState.mean().segment(m_pimpl->stateHandler.getVariable("tau_m").offset, m_pimpl->stateHandler.getVariable("tau_m").size)); -// log()->info("tau F\n{}", m_pimpl->predictedState.mean().segment(m_pimpl->stateHandler.getVariable("tau_F").offset, m_pimpl->stateHandler.getVariable("tau_F").size)); - - // Step 2 --> Set measurement - if (!m_pimpl->ukfCorrection->freeze_measurements(m_pimpl->ukfMeasurementFromSensors)) - { - log()->error("{} Cannot set the measurement to the UkfCorrection object.", logPrefix); - return false; - } - - // Step 3 --> Correct - m_pimpl->ukfCorrection->correct(m_pimpl->predictedState, m_pimpl->correctedState); - - m_pimpl->isValid = true; - - return true; -} - -bool RobotDynamicsEstimator::setInput(const RobotDynamicsEstimatorInput & input) -{ - constexpr auto logPrefix = "[RobotDynamicsEstimator::setInput]"; - - // Set the input provider state - m_pimpl->ukfInput.robotBasePose = input.basePose; - m_pimpl->ukfInput.robotBaseVelocity = input.baseVelocity; - m_pimpl->ukfInput.robotBaseAcceleration = input.baseAcceleration; - m_pimpl->ukfInput.robotJointPositions = input.jointPositions; - m_pimpl->ukfInput.robotJointAccelerations.setZero(); - - if (!m_pimpl->inputProvider->setInput(m_pimpl->ukfInput)) - { - log()->error("{} Cannot set the input of the input provider.", logPrefix); - return false; - } - - // Set the `std::map` used as measurement object - // for the freeze method of the UkfCorrection - m_pimpl->ukfMeasurementFromSensors["ds"] = input.jointVelocities; - m_pimpl->ukfMeasurementFromSensors["i_m"] = input.motorCurrents; - m_pimpl->ukfMeasurementFromSensors["dv_base"] = input.baseAcceleration.coeffs(); - for (auto & [key, value] : input.ftWrenches) - { - m_pimpl->ukfMeasurementFromSensors[key] = value; - } - for (auto & [key, value] : input.linearAccelerations) - { - m_pimpl->ukfMeasurementFromSensors[key] = value; - } - for (auto & [key, value] : input.angularVelocities) - { - m_pimpl->ukfMeasurementFromSensors[key] = value; - } - m_pimpl->ukfMeasurementFromSensors["tau_F"] = input.friction; - - return true; -} - -const RobotDynamicsEstimatorOutput& RobotDynamicsEstimator::getOutput() const -{ - constexpr auto logPrefix = "[RobotDynamicsEstimator::getOutput]"; - - if (m_pimpl->isValid) - { - m_pimpl->estimatorOutput.ds = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable("ds").offset, - m_pimpl->stateHandler.getVariable("ds").size); - - m_pimpl->estimatorOutput.tau_m = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable("tau_m").offset, - m_pimpl->stateHandler.getVariable("tau_m").size); - - m_pimpl->estimatorOutput.tau_F = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable("tau_F").offset, - m_pimpl->stateHandler.getVariable("tau_F").size); - - for (auto & [key, value] : m_pimpl->estimatorOutput.ftWrenches) - { - if (m_pimpl->stateHandler.getVariable(key).size > 0) - { - m_pimpl->estimatorOutput.ftWrenches[key] = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable(key).offset, - m_pimpl->stateHandler.getVariable(key).size); - } - else - { - log()->debug("{} Variable {} not found in the state vector.", logPrefix, key); - } - } - - for (auto & [key, value] : m_pimpl->estimatorOutput.ftWrenchesBiases) - { - if (m_pimpl->stateHandler.getVariable(key).size > 0) - { - m_pimpl->estimatorOutput.ftWrenchesBiases[key] = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable(key).offset, - m_pimpl->stateHandler.getVariable(key).size); - } - else - { - log()->debug("{} Variable {} not found in the state vector.", logPrefix, key); - } - } - - for (auto & [key, value] : m_pimpl->estimatorOutput.accelerometerBiases) - { - if (m_pimpl->stateHandler.getVariable(key).size > 0) - { - m_pimpl->estimatorOutput.accelerometerBiases[key] = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable(key).offset, - m_pimpl->stateHandler.getVariable(key).size); - } - else - { - log()->debug("{} Variable {} not found in the state vector.", logPrefix, key); - } - } - - for (auto & [key, value] : m_pimpl->estimatorOutput.gyroscopeBiases) - { - if (m_pimpl->stateHandler.getVariable(key).size > 0) - { - m_pimpl->estimatorOutput.gyroscopeBiases[key] = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable(key).offset, - m_pimpl->stateHandler.getVariable(key).size); - } - else - { - log()->debug("{} Variable {} not found in the state vector.", logPrefix, key); - } - } - - for (auto & [key, value] : m_pimpl->estimatorOutput.contactWrenches) - { - if (m_pimpl->stateHandler.getVariable(key).size > 0) - { - m_pimpl->estimatorOutput.contactWrenches[key] = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable(key).offset, - m_pimpl->stateHandler.getVariable(key).size); - } - else - { - log()->debug("{} Variable {} not found in the state vector.", logPrefix, key); - } - } - } - - return m_pimpl->estimatorOutput; -} diff --git a/src/Estimators/src/UkfMeasurement.cpp b/src/Estimators/src/UkfMeasurement.cpp deleted file mode 100644 index e8bd72352a..0000000000 --- a/src/Estimators/src/UkfMeasurement.cpp +++ /dev/null @@ -1,460 +0,0 @@ -/** - * @file UkfMeasurement.cpp - * @authors Ines Sorrentino - * @copyright 2023 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 - -#include -#include -#include -#include -#include -#include -#include - -using namespace BipedalLocomotion; -using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; - -using namespace std::chrono; - -//struct UkfMeasurement::Impl : public UkfModel { -// bfl::VectorDescription measurementDescription; -// bfl::VectorDescription inputDescription; -// Eigen::MatrixXd covarianceR; /**< Covariance matrix. */ -// int measurementSize{0}; /**< Length of the measurement vector. */ -// System::VariablesHandler measurementVariableHandler; /**< Variable handler describing the -// measurement vector. */ -// Eigen::VectorXd tempPredictedMeas; -// Eigen::MatrixXd predictedMeasurement; /**< Vector containing the updated measurement. */ -//}; - -//UkfMeasurement::UkfMeasurement() -//{ -// m_pimpl = std::make_unique(); -//} - -//UkfMeasurement::~UkfMeasurement() = default; - -bool UkfMeasurement::initialize(std::weak_ptr handler) -{ - constexpr auto logPrefix = "[UkfMeasurement::initialize]"; - - auto ptr = handler.lock(); - if (ptr == nullptr) - { - BipedalLocomotion::log()->error("{} The parameter handler is not valid.", logPrefix); - return false; - } - - if (!ptr->getParameter("sampling_time", m_dT)) - { - BipedalLocomotion::log()->error("{} Unable to find the `sampling_time` variable", - logPrefix); - return false; - } - - m_isInitialized = true; - - return true; -} - -bool UkfMeasurement::finalize(const System::VariablesHandler& handler) -{ - constexpr auto logPrefix = "[UkfMeasurement::finalize]"; - - if (!m_isInitialized) - { - BipedalLocomotion::log()->error("{} Please call initialize() before finalize().", - logPrefix); - return false; - } - - m_isFinalized = false; - - m_measurementSize = 0; - - // finalize all the dynamics - for (int indexDyn1 = 0; indexDyn1 < m_dynamicsList.size(); indexDyn1++) - { - if (!m_dynamicsList[indexDyn1].second->finalize(handler)) - { - BipedalLocomotion::log()->error("{} Error while finalizing the dynamics named {}", - logPrefix, - m_dynamicsList[indexDyn1].first); - return false; - } - - m_measurementSize += m_dynamicsList[indexDyn1].second->size(); - } - - // Set value of measurement covariance matrix - m_covarianceR.resize(m_measurementSize, m_measurementSize); - m_covarianceR.setZero(); - - for (int indexDyn2 = 0; indexDyn2 < m_dynamicsList.size(); indexDyn2++) - { - m_measurementVariableHandler - .addVariable(m_dynamicsList[indexDyn2].first, - m_dynamicsList[indexDyn2].second->getCovariance().size()); - - m_covarianceR.block(m_measurementVariableHandler - .getVariable(m_dynamicsList[indexDyn2].first) - .offset, - m_measurementVariableHandler - .getVariable(m_dynamicsList[indexDyn2].first) - .offset, - m_measurementVariableHandler - .getVariable(m_dynamicsList[indexDyn2].first) - .size, - m_measurementVariableHandler - .getVariable(m_dynamicsList[indexDyn2].first) - .size) - = m_dynamicsList[indexDyn2].second->getCovariance().asDiagonal(); - } - - m_jointVelocityState.resize(m_kinDynFullModel->model().getNrOfDOFs()); - m_jointAccelerationState.resize(m_kinDynFullModel->model().getNrOfDOFs()); - - for (int idx = 0; idx < m_subModelList.size(); idx++) - { - m_subModelJointVel.emplace_back( - Eigen::VectorXd(m_subModelList[idx].getModel().getNrOfDOFs())); - m_subModelJointAcc.emplace_back( - Eigen::VectorXd(m_subModelList[idx].getModel().getNrOfDOFs())); - m_subModelJointPos.emplace_back( - Eigen::VectorXd(m_subModelList[idx].getModel().getNrOfDOFs())); - m_subModelJointMotorTorque.emplace_back( - Eigen::VectorXd(m_subModelList[idx].getModel().getNrOfDOFs())); - m_subModelFrictionTorque.emplace_back( - Eigen::VectorXd(m_subModelList[idx].getModel().getNrOfDOFs())); - m_totalTorqueFromContacts.emplace_back( - Eigen::VectorXd(6 + m_subModelList[idx].getModel().getNrOfDOFs())); - m_tempJacobianList.emplace_back( - Eigen::MatrixXd(6, 6 + m_subModelList[idx].getModel().getNrOfDOFs())); - m_subModelNuDot.emplace_back( - Eigen::VectorXd(6 + m_subModelList[idx].getModel().getNrOfDOFs())); - } - - m_currentState.resize(handler.getNumberOfVariables()); - m_currentState.setZero(); - - m_measurement.resize(m_measurementSize); - m_measurement.setZero(); - - m_tempPredictedMeas.resize(m_measurementSize); - m_tempPredictedMeas.setZero(); - - m_measurementDescription = bfl::VectorDescription(m_measurementSize); - - m_predictedMeasurement.resize(m_measurementSize, 2 * handler.getNumberOfVariables() + 1); - m_predictedMeasurement.setZero(); - - m_isFinalized = true; - - return true; -} - -std::unique_ptr -UkfMeasurement::build(std::weak_ptr handler, - System::VariablesHandler& stateVariableHandler, - std::shared_ptr kinDynFullModel, - const std::vector& subModelList, - const std::vector>& kinDynWrapperList) -{ - constexpr auto logPrefix = "[UkfMeasurement::build]"; - - auto ptr = handler.lock(); - if (ptr == nullptr) - { - BipedalLocomotion::log()->error("{} Invalid parameter handler.", logPrefix); - return nullptr; - } - - std::unique_ptr measurement = std::make_unique(); - - if (!measurement->initialize(ptr)) - { - BipedalLocomotion::log()->error("{} Unable to initialize the measurement object of the " - "ukf.", - logPrefix); - return nullptr; - } - - std::vector dynamicsList; - if (!ptr->getParameter("dynamics_list", dynamicsList)) - { - BipedalLocomotion::log()->error("{} Unable to find the parameter 'dynamics_list'.", - logPrefix); - return nullptr; - } - - if (kinDynFullModel == nullptr) - { - BipedalLocomotion::log()->error("{} The pointer to the `KinDynComputation` object is not " - "valid.", - logPrefix); - return nullptr; - } - - measurement->m_stateVariableHandler = stateVariableHandler; - - // Set KinDynComputation for the full model - measurement->m_kinDynFullModel = kinDynFullModel; - - // Set the list of SubModel - measurement->m_subModelList.reserve(subModelList.size()); - measurement->m_subModelList = subModelList; - - // set the list of KinDynWrapper - measurement->m_kinDynWrapperList.reserve(kinDynWrapperList.size()); - measurement->m_kinDynWrapperList = kinDynWrapperList; - - // per each dynamics add variable to the variableHandler - // and add the dynamics to the list - for (const auto& dynamicsGroupName : dynamicsList) - { - auto dynamicsGroupTmp = ptr->getGroup(dynamicsGroupName).lock(); - if (dynamicsGroupTmp == nullptr) - { - BipedalLocomotion::log()->error("{} Unable to find the group '{}'.", - logPrefix, - dynamicsGroupName); - return nullptr; - } - - // add dT parameter since it is required by all the dynamics - auto dynamicsGroup = dynamicsGroupTmp->clone(); - dynamicsGroup->setParameter("sampling_time", measurement->m_dT); - - // create variable handler - std::string dynamicsName; - std::vector covariances; - if (!dynamicsGroup->getParameter("name", dynamicsName)) - { - BipedalLocomotion::log()->error("{} Unable to find the parameter 'name'.", logPrefix); - return nullptr; - } - if (!dynamicsGroup->getParameter("covariance", covariances)) - { - BipedalLocomotion::log()->error("{} Unable to find the parameter 'covariance'.", - logPrefix); - return nullptr; - } - - std::string dynamicModel; - if (!dynamicsGroup->getParameter("dynamic_model", dynamicModel)) - { - BipedalLocomotion::log()->error("{} Unable to find the parameter 'dynamic_model'.", - logPrefix); - return nullptr; - } - - std::shared_ptr dynamicsInstance = DynamicsFactory::createInstance(dynamicModel); - if (dynamicsInstance == nullptr) - { - BipedalLocomotion::log()->error("{} The dynamic model '{}' has not been registered for " - "the measurement variable `{}`.", - logPrefix, - dynamicModel, - dynamicsName); - return nullptr; - } - - dynamicsInstance->setSubModels(subModelList, kinDynWrapperList); - - dynamicsInstance->initialize(dynamicsGroup); - - // add dynamics to the list - // measurement->dynamicsList.insert({dynamicsName, dynamicsInstance}); - measurement->m_dynamicsList.emplace_back(dynamicsName, dynamicsInstance); - } - - measurement->m_inputDescription - = bfl::VectorDescription(stateVariableHandler.getNumberOfVariables()); - - // finalize estimator - if (!measurement->finalize(measurement->m_stateVariableHandler)) - { - BipedalLocomotion::log()->error("{} Unable to finalize the RobotDynamicsEstimator.", - logPrefix); - return nullptr; - } - - return measurement; -} - -void UkfMeasurement::setUkfInputProvider(std::shared_ptr ukfInputProvider) -{ - m_ukfInputProvider = ukfInputProvider; -} - -std::pair UkfMeasurement::getNoiseCovarianceMatrix() const -{ - return std::make_pair(true, m_covarianceR); -} - -bfl::VectorDescription UkfMeasurement::getMeasurementDescription() const -{ - return m_measurementDescription; -} - -bfl::VectorDescription UkfMeasurement::getInputDescription() const -{ - return m_inputDescription; -} - -// TODO -// Here the cur_state has size state_size x n_sigma_points -// this means that the computation can be parallelized -std::pair -UkfMeasurement::predictedMeasure(const Eigen::Ref& cur_states) const -{ - constexpr auto logPrefix = "[UkfMeasurement::propagate]"; - - // Check that everything is initialized and set - if (!m_isFinalized) - { - BipedalLocomotion::log()->error("{} The ukf state is not well initialized.", logPrefix); - throw std::runtime_error("Error"); - } - - if (m_ukfInputProvider == nullptr) - { - BipedalLocomotion::log()->error("{} The ukf input provider is not set.", logPrefix); - throw std::runtime_error("Error"); - } - - // Get input of ukf from provider - const_cast(this)->m_ukfInput = m_ukfInputProvider->getOutput(); - - for (int sample = 0; sample < cur_states.cols(); sample++) - { - const_cast(this)->m_currentState = cur_states.col(sample); - - const_cast(this)->unpackState(); - - if (!const_cast(this)->updateState()) - { - BipedalLocomotion::log()->error("{} The joint accelerations are not updated.", - logPrefix); - throw std::runtime_error("Error"); - } - - const_cast(this)->m_ukfInput.robotJointAccelerations = m_jointAccelerationState; - - // BipedalLocomotion::log()->info("Joint acceleration\n{}", - // ukfInput.robotJointAccelerations); - - // TODO - // This could be parallelized - for (int indexDyn = 0; indexDyn < m_dynamicsList.size(); indexDyn++) - { - m_dynamicsList[indexDyn].second->setState(m_currentState); - - m_dynamicsList[indexDyn].second->setInput(m_ukfInput); - - if (!m_dynamicsList[indexDyn].second->update()) - { - BipedalLocomotion::log()->error("{} Cannot update the dynamics with name `{}`.", - logPrefix, - m_dynamicsList[indexDyn].first); - throw std::runtime_error("Error"); - } - - const_cast(this)->m_tempPredictedMeas - .segment(m_measurementVariableHandler - .getVariable(m_dynamicsList[indexDyn].first) - .offset, - m_measurementVariableHandler - .getVariable(m_dynamicsList[indexDyn].first) - .size) - = m_dynamicsList[indexDyn].second->getUpdatedVariable(); - } - const_cast(this)->m_predictedMeasurement.col(sample) = m_tempPredictedMeas; - } - - // BipedalLocomotion::log()->info("Predicted measurement sigma points"); - // BipedalLocomotion::log()->info(predictedMeasurement.transpose()); - - return std::make_pair(true, m_predictedMeasurement); -} - -std::pair UkfMeasurement::innovation(const bfl::Data& predicted_measurements, - const bfl::Data& measurements) const -{ - Eigen::MatrixXd innovation - = -(bfl::any::any_cast(predicted_measurements).colwise() - - bfl::any::any_cast(measurements)); - - return std::make_pair(true, std::move(innovation)); -} - -std::size_t UkfMeasurement::getMeasurementSize() -{ - return m_measurementSize; -} - -System::VariablesHandler& UkfMeasurement::getMeasurementVariableHandler() -{ - return m_measurementVariableHandler; -} - -void UkfMeasurement::setStateVariableHandler(System::VariablesHandler stateVariableHandler) -{ - m_stateVariableHandler = stateVariableHandler; -} - -std::pair UkfMeasurement::measure(const bfl::Data& /**data**/) const -{ - return std::make_pair(true, m_measurement); -} - -bool UkfMeasurement::freeze(const bfl::Data& data) -{ - constexpr auto logPrefix = "[UkfMeasurement::freeze]"; - - m_measurementMap = bfl::any::any_cast>(data); - - for (int index = 0; index < m_dynamicsList.size(); index++) - { - m_offsetMeasurement - = m_measurementVariableHandler.getVariable(m_dynamicsList[index].first) - .offset; - - if (m_measurementMap.count(m_dynamicsList[index].first) == 0) - { - BipedalLocomotion::log()->error("{} Measurement with name `{}` not found.", - logPrefix, - m_dynamicsList[index].first); - return false; - } - - // If more sub-models share the same accelerometer or gyroscope sensor, the measurement - // vector is concatenated a number of times equal to the number of sub-models using the - // sensor. - while ( - m_offsetMeasurement - < (m_measurementVariableHandler.getVariable(m_dynamicsList[index].first) - .offset - + m_measurementVariableHandler.getVariable(m_dynamicsList[index].first) - .size)) - { - m_measurement - .segment(m_offsetMeasurement, - m_measurementMap[m_dynamicsList[index].first].size()) - = m_measurementMap[m_dynamicsList[index].first]; - - m_offsetMeasurement - += m_measurementMap[m_dynamicsList[index].first].size(); - } - } - - return true; -} diff --git a/src/Estimators/src/UkfModel.cpp b/src/Estimators/src/UkfModel.cpp deleted file mode 100644 index b9f5e86c69..0000000000 --- a/src/Estimators/src/UkfModel.cpp +++ /dev/null @@ -1,198 +0,0 @@ -/** - * @file UkfModel.cpp - * @authors Ines Sorrentino - * @copyright 2023 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 - -#include - -using namespace BipedalLocomotion; -using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; - -void UkfModel::unpackState() -{ - m_jointVelocityState = m_currentState.segment(m_stateVariableHandler.getVariable("ds").offset, - m_stateVariableHandler.getVariable("ds").size); - - for (int subModelIdx = 0; subModelIdx < m_subModelList.size(); subModelIdx++) - { - // Take sub-model joint velocities, motor torques, friction torques, ft wrenches, ext - // contact wrenches - for (int jointIdx = 0; jointIdx < m_subModelList[subModelIdx].getModel().getNrOfDOFs(); - jointIdx++) - { - m_subModelJointVel[subModelIdx](jointIdx) - = m_jointVelocityState(m_subModelList[subModelIdx].getJointMapping()[jointIdx]); - - m_subModelJointMotorTorque[subModelIdx](jointIdx) - = m_currentState[m_stateVariableHandler.getVariable("tau_m").offset - + m_subModelList[subModelIdx].getJointMapping()[jointIdx]]; - - m_subModelFrictionTorque[subModelIdx](jointIdx) - = m_currentState[m_stateVariableHandler.getVariable("tau_F").offset - + m_subModelList[subModelIdx].getJointMapping()[jointIdx]]; - } - - for (auto& [key, value] : m_subModelList[subModelIdx].getFTList()) - { - m_FTMap[key] = m_currentState.segment(m_stateVariableHandler.getVariable(key).offset, - m_stateVariableHandler.getVariable(key).size); - } - - for (auto& [key, value] : m_subModelList[subModelIdx].getExternalContactList()) - { - m_extContactMap[key] - = m_currentState.segment(m_stateVariableHandler.getVariable(key).offset, - m_stateVariableHandler.getVariable(key).size); - } - - for (auto& [key, value] : m_subModelList[subModelIdx].getAccelerometerList()) - { - m_accBiasMap[key + "_bias"] - = m_currentState.segment(m_stateVariableHandler.getVariable(key).offset, - m_stateVariableHandler.getVariable(key).size); - } - - for (auto& [key, value] : m_subModelList[subModelIdx].getGyroscopeList()) - { - m_gyroBiasMap[key + "_bias"] - = m_currentState.segment(m_stateVariableHandler.getVariable(key).offset, - m_stateVariableHandler.getVariable(key).size); - } - } -} - -bool UkfModel::updateState() -{ - constexpr auto logPrefix = "[UkfModel::updateState]"; - - // Update kindyn full model - m_kinDynFullModel->setRobotState(m_ukfInput.robotBasePose.transform(), - m_ukfInput.robotJointPositions, - iDynTree::make_span(m_ukfInput.robotBaseVelocity.data(), - manif::SE3d::Tangent::DoF), - m_jointVelocityState, - m_gravity); - - // TODO - // Devi aggiornare lo stato dei kindyn associati ai sottomodelli e devi finire di correggere - // il giroscopio misura modello - - // compute joint acceleration per each sub-model - for (int subModelIdx = 0; subModelIdx < m_subModelList.size(); subModelIdx++) - { - // Get sub-model base pose - m_worldTBase = Conversions::toManifPose(m_kinDynFullModel->getWorldTransform( - m_kinDynWrapperList[subModelIdx]->getFloatingBase())); - - // Get sub-model joint position - for (int jointIdx = 0; jointIdx < m_subModelList[subModelIdx].getModel().getNrOfDOFs(); - jointIdx++) - { - m_subModelJointPos[subModelIdx](jointIdx) - = m_ukfInput - .robotJointPositions[m_subModelList[subModelIdx].getJointMapping()[jointIdx]]; - } - - // Get sub-model base vel - if (!m_kinDynFullModel->getFrameVel(m_kinDynWrapperList[subModelIdx]->getFloatingBase(), - m_subModelBaseVelTemp)) - { - log()->error("{} Failed while getting the base frame velocity.", logPrefix); - return false; - } - - // Set the sub-model state - m_kinDynWrapperList[subModelIdx] - ->setRobotState(m_worldTBase.transform(), - m_subModelJointPos[subModelIdx], - iDynTree::make_span(m_subModelBaseVelTemp.data(), - manif::SE3d::Tangent::DoF), - m_subModelJointVel[subModelIdx], - m_gravity); - - m_totalTorqueFromContacts[subModelIdx].setZero(); - - // Contribution of FT measurements - for (const auto& [key, value] : m_subModelList[subModelIdx].getFTList()) - { - m_wrench = (int)value.forceDirection * m_FTMap[key].array(); - - if (!m_kinDynWrapperList[subModelIdx] - ->getFrameFreeFloatingJacobian(value.index, m_tempJacobianList[subModelIdx])) - { - log()->error("{} Failed while getting the jacobian for the frame `{}`.", logPrefix, value.frame); - return false; - } - - m_totalTorqueFromContacts[subModelIdx] - += m_tempJacobianList[subModelIdx].transpose() * m_wrench; - } - - // Contribution of unknown external contacts - for (auto& [key, value] : m_subModelList[subModelIdx].getExternalContactList()) - { - if (!m_kinDynWrapperList[subModelIdx] - ->getFrameFreeFloatingJacobian(value, m_tempJacobianList[subModelIdx])) - { - log()->error("{} Failed while getting the jacobian for the frame `{}`.", logPrefix, value); - return false; - } - - m_totalTorqueFromContacts[subModelIdx] - += m_tempJacobianList[subModelIdx].transpose() * m_extContactMap[key]; - } - - if (subModelIdx == 0) - { - if (!m_kinDynWrapperList[subModelIdx] - ->forwardDynamics(m_subModelJointMotorTorque[subModelIdx], - m_subModelFrictionTorque[subModelIdx], - m_totalTorqueFromContacts[subModelIdx].tail( - m_kinDynWrapperList[subModelIdx] - ->getNrOfDegreesOfFreedom()), - m_ukfInput.robotBaseAcceleration.coeffs(), - m_subModelJointAcc[subModelIdx])) - { - log()->error("{} Cannot compute the inverse dynamics.", logPrefix); - return false; - } - - m_subModelNuDot[subModelIdx].head(6) = m_ukfInput.robotBaseAcceleration.coeffs(); - m_subModelNuDot[subModelIdx].tail( - m_kinDynWrapperList[subModelIdx]->getNrOfDegreesOfFreedom()) - = m_subModelJointAcc[subModelIdx]; - } else - { - if (!m_kinDynWrapperList[subModelIdx] - ->forwardDynamics(m_subModelJointMotorTorque[subModelIdx], - m_subModelFrictionTorque[subModelIdx], - m_totalTorqueFromContacts[subModelIdx], - m_subModelNuDot[subModelIdx])) - { - log()->error("{} Cannot compute the inverse dynamics.", logPrefix); - return false; - } - - m_subModelJointAcc[subModelIdx] = m_subModelNuDot[subModelIdx].tail( - m_kinDynWrapperList[subModelIdx]->getNrOfDegreesOfFreedom()); - } - - // Assign joint acceleration using the correct indeces - for (int jointIdx = 0; jointIdx < m_subModelList[subModelIdx].getJointMapping().size(); - jointIdx++) - { - m_jointAccelerationState[m_subModelList[subModelIdx].getJointMapping()[jointIdx]] - = m_subModelJointAcc[subModelIdx][jointIdx]; - } - } - - return true; -} diff --git a/src/Estimators/src/UkfState.cpp b/src/Estimators/src/UkfState.cpp deleted file mode 100644 index 3bb6e106b2..0000000000 --- a/src/Estimators/src/UkfState.cpp +++ /dev/null @@ -1,351 +0,0 @@ -/** - * @file UkfState.cpp - * @authors Ines Sorrentino - * @copyright 2023 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 - -#include -#include -#include -#include -#include -#include - -using namespace BipedalLocomotion; -namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; - -using namespace std::chrono; - -bool RDE::UkfState::initialize(std::weak_ptr handler) -{ - constexpr auto logPrefix = "[UkfState::initialize]"; - - auto ptr = handler.lock(); - if (ptr == nullptr) - { - log()->error("{} The parameter handler is not valid.", logPrefix); - return false; - } - - if (!ptr->getParameter("sampling_time", m_dT)) - { - log()->error("{} Unable to find the `sampling_time` variable", logPrefix); - return false; - } - - m_isInitialized = true; - - return true; -} - -bool RDE::UkfState::finalize(const System::VariablesHandler& handler) -{ - constexpr auto logPrefix = "[UkfState::finalize]"; - - if (!m_isInitialized) - { - log()->error("{} Please call initialize() before finalize().", logPrefix); - return false; - } - - m_isFinalized = false; - - m_stateSize = 0; - - for (int indexDyn1 = 0; indexDyn1 < m_dynamicsList.size(); indexDyn1++) - { - if (!m_dynamicsList[indexDyn1].second->finalize(handler)) - { - log()->error("{} Error while finalizing the dynamics named {}", - logPrefix, - m_dynamicsList[indexDyn1].first); - return false; - } - - m_stateSize += m_dynamicsList[indexDyn1].second->size(); - } - - // Set value of process covariance matrix - m_covarianceQ.resize(m_stateSize, m_stateSize); - m_covarianceQ.setZero(); - - m_initialCovariance.resize(m_stateSize, m_stateSize); - m_initialCovariance.setZero(); - - for (int indexDyn2 = 0; indexDyn2 < m_dynamicsList.size(); indexDyn2++) - { - m_covarianceQ - .block(handler.getVariable(m_dynamicsList[indexDyn2].first).offset, - handler.getVariable(m_dynamicsList[indexDyn2].first).offset, - handler.getVariable(m_dynamicsList[indexDyn2].first).size, - handler.getVariable(m_dynamicsList[indexDyn2].first).size) - = m_dynamicsList[indexDyn2].second->getCovariance().asDiagonal(); - - m_initialCovariance - .block(handler.getVariable(m_dynamicsList[indexDyn2].first).offset, - handler.getVariable(m_dynamicsList[indexDyn2].first).offset, - handler.getVariable(m_dynamicsList[indexDyn2].first).size, - handler.getVariable(m_dynamicsList[indexDyn2].first).size) - = m_dynamicsList[indexDyn2].second->getInitialStateCovariance().asDiagonal(); - } - - // log()->info("Covariance process\n{}", m_covarianceQ); - - m_jointVelocityState.resize(m_kinDynFullModel->model().getNrOfDOFs()); - m_jointAccelerationState.resize(m_kinDynFullModel->model().getNrOfDOFs()); - - for (int idx = 0; idx < m_subModelList.size(); idx++) - { - m_subModelJointVel.emplace_back( - Eigen::VectorXd(m_subModelList[idx].getModel().getNrOfDOFs())); - m_subModelJointAcc.emplace_back( - Eigen::VectorXd(m_subModelList[idx].getModel().getNrOfDOFs())); - m_subModelJointPos.emplace_back( - Eigen::VectorXd(m_subModelList[idx].getModel().getNrOfDOFs())); - m_subModelJointMotorTorque.emplace_back( - Eigen::VectorXd(m_subModelList[idx].getModel().getNrOfDOFs())); - m_subModelFrictionTorque.emplace_back( - Eigen::VectorXd(m_subModelList[idx].getModel().getNrOfDOFs())); - m_totalTorqueFromContacts.emplace_back( - Eigen::VectorXd(6 + m_subModelList[idx].getModel().getNrOfDOFs())); - m_tempJacobianList.emplace_back( - Eigen::MatrixXd(6, 6 + m_subModelList[idx].getModel().getNrOfDOFs())); - m_subModelNuDot.emplace_back( - Eigen::VectorXd(6 + m_subModelList[idx].getModel().getNrOfDOFs())); - } - - m_currentState.resize(m_stateSize); - m_currentState.setZero(); - - m_nextState.resize(m_stateSize); - m_nextState.setZero(); - - m_isFinalized = true; - - return true; -} - -std::unique_ptr -RDE::UkfState::build(std::weak_ptr handler, - std::shared_ptr kinDynFullModel, - const std::vector& subModelList, - const std::vector>& kinDynWrapperList) -{ - constexpr auto logPrefix = "[UkfState::build]"; - - auto ptr = handler.lock(); - if (ptr == nullptr) - { - log()->error("{} Invalid parameter handler.", logPrefix); - return nullptr; - } - - std::unique_ptr state = std::make_unique(); - - if (!state->initialize(ptr)) - { - log()->error("{} Unable to initialize the state object of the ukf.", logPrefix); - return nullptr; - } - - std::vector dynamicsList; - if (!ptr->getParameter("dynamics_list", dynamicsList)) - { - log()->error("{} Unable to find the parameter 'dynamics_list'.", logPrefix); - return nullptr; - } - - if (kinDynFullModel == nullptr) - { - log()->error("{} The pointer to the `KinDynComputation` object is not valid.", logPrefix); - return nullptr; - } - - // Set KinDynComputation for the full model - state->m_kinDynFullModel = kinDynFullModel; - - // Set the list of SubModel - state->m_subModelList.reserve(subModelList.size()); - state->m_subModelList = subModelList; - - // set the list of KinDynWrapper - state->m_kinDynWrapperList.reserve(kinDynWrapperList.size()); - state->m_kinDynWrapperList = kinDynWrapperList; - - // per each dynamics add variable to the variableHandler - // and add the dynamics to the list - for (const auto& dynamicsGroupName : dynamicsList) - { - auto dynamicsGroupTmp = ptr->getGroup(dynamicsGroupName).lock(); - if (dynamicsGroupTmp == nullptr) - { - log()->error("{} Unable to find the group '{}'.", logPrefix, dynamicsGroupName); - return nullptr; - } - - // add dT parameter since it is required by all the dynamics - auto dynamicsGroup = dynamicsGroupTmp->clone(); - dynamicsGroup->setParameter("sampling_time", state->m_dT); - - // create variable handler - std::string dynamicsName; - std::vector covariances; - if (!dynamicsGroup->getParameter("name", dynamicsName)) - { - log()->error("{} Unable to find the parameter 'name'.", logPrefix); - return nullptr; - } - if (!dynamicsGroup->getParameter("covariance", covariances)) - { - log()->error("{} Unable to find the parameter 'covariance'.", logPrefix); - return nullptr; - } - if (!dynamicsGroup->getParameter("initial_covariance", covariances)) - { - log()->error("{} Unable to find the parameter 'initial_covariance'.", logPrefix); - return nullptr; - } - state->m_stateVariableHandler.addVariable(dynamicsName, covariances.size()); - - std::string dynamicModel; - if (!dynamicsGroup->getParameter("dynamic_model", dynamicModel)) - { - log()->error("{} Unable to find the parameter 'dynamic_model'.", logPrefix); - return nullptr; - } - - std::shared_ptr dynamicsInstance - = RDE::DynamicsFactory::createInstance(dynamicModel); - if (dynamicsInstance == nullptr) - { - log()->error("{} The dynamic model '{}' has not been registered for the state variable " - "`{}`.", - logPrefix, - dynamicModel, - dynamicsName); - return nullptr; - } - - dynamicsInstance->setSubModels(subModelList, kinDynWrapperList); - - dynamicsInstance->initialize(dynamicsGroup); - - // add dynamics to the list - state->m_dynamicsList.emplace_back(dynamicsName, dynamicsInstance); - } - - // finalize estimator - if (!state->finalize(state->m_stateVariableHandler)) - { - log()->error("{} Unable to finalize the RobotDynamicsEstimator.", logPrefix); - return nullptr; - } - - return state; -} - -void RDE::UkfState::setUkfInputProvider(std::shared_ptr ukfInputProvider) -{ - m_ukfInputProvider = ukfInputProvider; -} - -Eigen::MatrixXd RDE::UkfState::getNoiseCovarianceMatrix() -{ - return m_covarianceQ; -} - -System::VariablesHandler& RDE::UkfState::getStateVariableHandler() -{ - return m_stateVariableHandler; -} - -// TODO -// Here the cur_state has size state_size x n_sigma_points -// this means that the computation can be parallelized -void RDE::UkfState::propagate(const Eigen::Ref& cur_states, - Eigen::Ref prop_states) -{ - constexpr auto logPrefix = "[UkfState::propagate]"; - - // Check that everything is initialized and set - if (!m_isFinalized) - { - log()->error("{} The ukf state is not well initialized.", logPrefix); - throw std::runtime_error("Error"); - } - - if (m_ukfInputProvider == nullptr) - { - log()->error("{} The ukf input provider is not set.", logPrefix); - throw std::runtime_error("Error"); - } - - // Get input of ukf from provider - m_ukfInput = m_ukfInputProvider->getOutput(); - - prop_states.resize(cur_states.rows(), cur_states.cols()); - - for (int sample = 0; sample < cur_states.cols(); sample++) - { - m_currentState = cur_states.col(sample); - - unpackState(); - - if (!updateState()) - { - log()->error("{} The joint accelerations are not updated.", logPrefix); - throw std::runtime_error("Error"); - } - - m_ukfInput.robotJointAccelerations = m_jointAccelerationState; - - // TODO - // This could be parallelized - for (int indexDyn = 0; indexDyn < m_dynamicsList.size(); indexDyn++) - { - m_dynamicsList[indexDyn].second->setState(m_currentState); - - m_dynamicsList[indexDyn].second->setInput(m_ukfInput); - - if (!m_dynamicsList[indexDyn].second->update()) - { - log()->error("{} Cannot update the dynamics with name `{}`.", - logPrefix, - m_dynamicsList[indexDyn].first); - throw std::runtime_error("Error"); - } - - m_nextState.segment(m_stateVariableHandler - .getVariable(m_dynamicsList[indexDyn].first) - .offset, - m_stateVariableHandler - .getVariable(m_dynamicsList[indexDyn].first) - .size) - = m_dynamicsList[indexDyn].second->getUpdatedVariable(); - } - - prop_states.col(sample) = m_nextState; - } -} - -bfl::VectorDescription RDE::UkfState::getStateDescription() -{ - return bfl::VectorDescription(m_stateSize); -} - -std::size_t RDE::UkfState::getStateSize() -{ - return m_stateSize; -} - -Eigen::Ref RDE::UkfState::getInitialStateCovarianceMatrix() -{ - return m_initialCovariance; -} diff --git a/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt b/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt index e5c21770d6..0cb30d7cc6 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt +++ b/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt @@ -11,6 +11,13 @@ if(FRAMEWORK_USE_icub-models AND FRAMEWORK_COMPILE_YarpImplementation) BipedalLocomotion::ParametersHandlerYarpImplementation icub-models::icub-models) + add_bipedal_test(NAME KinDynWrapper + SOURCES KinDynWrapperTest.cpp + LINKS BipedalLocomotion::RobotDynamicsEstimator + BipedalLocomotion::ParametersHandler + BipedalLocomotion::ParametersHandlerYarpImplementation + icub-models::icub-models) + add_bipedal_test(NAME ZeroVelocityStateDynamics SOURCES ZeroVelocityStateDynamicsTest.cpp LINKS BipedalLocomotion::RobotDynamicsEstimator diff --git a/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp index 73bfdf2a10..2d8a0c9594 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp @@ -324,8 +324,9 @@ TEST_CASE("Gyroscope Measurement Dynamics") gyroDynamics.setState(state); // Get bias - Eigen::Vector3d bias = state.segment(stateVariableHandler.getVariable("r_leg_ft_gyro_bias").offset, - stateVariableHandler.getVariable("r_leg_ft_gyro_bias").size); + Eigen::Vector3d bias + = state.segment(stateVariableHandler.getVariable("r_leg_ft_gyro_bias").offset, + stateVariableHandler.getVariable("r_leg_ft_gyro_bias").size); // Update the dynamics REQUIRE(gyroDynamics.update()); @@ -337,7 +338,7 @@ TEST_CASE("Gyroscope Measurement Dynamics") for (int idx = 0; idx < gyroDynamics.getUpdatedVariable().size(); idx++) { REQUIRE(std::abs(gyroDynamics.getUpdatedVariable()(idx) - bias(idx) - - gyroFrameVel.coeffs()(idx+3)) + - gyroFrameVel.coeffs()(idx + 3)) < 0.1); } } diff --git a/src/Estimators/tests/RobotDynamicsEstimator/KinDynWrapperTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/KinDynWrapperTest.cpp new file mode 100644 index 0000000000..68d4ca1e4e --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/KinDynWrapperTest.cpp @@ -0,0 +1,230 @@ +/** + * @file KinDynWrapperTest.cpp + * @authors Ines Sorrentino + * @copyright 2023 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 +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; + +void createModelLoader(IParametersHandler::shared_ptr group, iDynTree::ModelLoader& mdlLdr) +{ + // List of joints and fts to load the model + std::vector subModelList; + + const std::string modelPath = iCubModels::getModelFile("iCubGenova09"); + + std::vector jointList; + REQUIRE(group->getParameter("joint_list", jointList)); + + std::vector ftFramesList; + auto ftGroup = group->getGroup("FT").lock(); + REQUIRE(ftGroup->getParameter("associated_joints", ftFramesList)); + + std::vector jointsAndFTs; + jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); + + REQUIRE(mdlLdr.loadReducedModelFromFile(modelPath, jointsAndFTs)); +} + +void createSubModels(iDynTree::ModelLoader& mdlLdr, + std::shared_ptr kinDynFullModel, + IParametersHandler::shared_ptr group, + SubModelCreator& subModelCreator) +{ + subModelCreator.setModelAndSensors(mdlLdr.model(), mdlLdr.sensors()); + REQUIRE(subModelCreator.setKinDyn(kinDynFullModel)); + + REQUIRE(subModelCreator.createSubModels(group)); +} + +bool setStaticState(std::shared_ptr kinDyn) +{ + size_t dofs = kinDyn->getNrOfDegreesOfFreedom(); + iDynTree::Transform worldTbase; + Eigen::VectorXd baseVel(6); + Eigen::Vector3d gravity; + + Eigen::VectorXd qj(dofs), dqj(dofs), ddqj(dofs); + + worldTbase = iDynTree::Transform(iDynTree::Rotation::Identity(), iDynTree::Position::Zero()); + + Eigen::Matrix4d transform = toEigen(worldTbase.asHomogeneousTransform()); + + gravity.setZero(); + gravity(2) = -BipedalLocomotion::Math::StandardAccelerationOfGravitation; + + baseVel.setZero(); + + for (size_t dof = 0; dof < dofs; dof++) + { + qj(dof) = 0.0; + dqj(dof) = 0.0; + ddqj(dof) = 0.0; + } + + 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())); +} + +IParametersHandler::shared_ptr createModelParameterHandler() +{ + // Create model variable handler to load the robot model + auto modelParamHandler = std::make_shared(); + + const std::vector jointList + = {"r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll"}; + + auto emptyGroupNamesFrames = std::make_shared(); + std::vector emptyVectorString; + emptyGroupNamesFrames->setParameter("names", emptyVectorString); + emptyGroupNamesFrames->setParameter("frames", emptyVectorString); + emptyGroupNamesFrames->setParameter("associated_joints", emptyVectorString); + REQUIRE(modelParamHandler->setGroup("FT", emptyGroupNamesFrames)); + REQUIRE(modelParamHandler->setGroup("GYROSCOPE", emptyGroupNamesFrames)); + REQUIRE(modelParamHandler->setGroup("ACCELEROMETER", emptyGroupNamesFrames)); + auto emptyGroupFrames = std::make_shared(); + emptyGroupFrames->setParameter("frames", emptyVectorString); + REQUIRE(modelParamHandler->setGroup("EXTERNAL_CONTACT", emptyGroupFrames)); + + modelParamHandler->setParameter("joint_list", jointList); + + return modelParamHandler; +} + +TEST_CASE("KinDynWrapper Test") +{ + // Create parameter handler to load the model + auto modelParamHandler = createModelParameterHandler(); + + // Load model + iDynTree::ModelLoader modelLoader; + createModelLoader(modelParamHandler, modelLoader); + + auto kinDyn = std::make_shared(); + REQUIRE(kinDyn->loadRobotModel(modelLoader.model())); + REQUIRE(kinDyn->setFrameVelocityRepresentation(iDynTree::BODY_FIXED_REPRESENTATION)); + + REQUIRE(setStaticState(kinDyn)); + + SubModelCreator subModelCreator; + createSubModels(modelLoader, kinDyn, modelParamHandler, subModelCreator); + + std::vector> kinDynWrapperList; + std::vector subModelList = subModelCreator.getSubModelList(); + + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + kinDynWrapperList.emplace_back(std::make_shared()); + REQUIRE(kinDynWrapperList[idx]->setModel(subModelList[idx])); + } + + // Test kindynwrapper + Eigen::Vector3d gravity; + gravity.setZero(); + gravity(2) = -BipedalLocomotion::Math::StandardAccelerationOfGravitation; + + int numJoints = 6; + + Eigen::VectorXd jointPos = Eigen::VectorXd::Random(numJoints); + Eigen::VectorXd jointVel = Eigen::VectorXd::Random(numJoints); + Eigen::VectorXd jointAcc = Eigen::VectorXd::Random(numJoints); + + manif::SE3d basePose + = BipedalLocomotion::Conversions::toManifPose(iDynTree::getRandomTransform()); + + manif::SE3Tangentd baseVel + = BipedalLocomotion::Conversions::toManifTwist(iDynTree::getRandomTwist()); + + manif::SE3Tangentd baseAcc + = BipedalLocomotion::Conversions::toManifTwist(iDynTree::getRandomTwist()); + + REQUIRE(kinDyn->setRobotState(basePose.transform(), + jointPos, + iDynTree::make_span(baseVel.data(), manif::SE3d::Tangent::DoF), + jointVel, + gravity)); + + // Compute joint torques from inverse dynamics on the full model + iDynTree::LinkNetExternalWrenches extWrench(kinDyn->model()); + extWrench.zero(); + iDynTree::FreeFloatingGeneralizedTorques jointTorques(kinDyn->model()); + kinDyn->inverseDynamics(iDynTree::make_span(baseAcc.data(), manif::SE3d::Tangent::DoF), + jointAcc, + extWrench, + jointTorques); + + Eigen::VectorXd jointTrq = iDynTree::toEigen(jointTorques.jointTorques()); + + // The submodel is only one and it is the full model + // Indeed the model does not have ft sensors in this test + // Get sub-model base pose + manif::SE3d worldTBase = BipedalLocomotion::Conversions::toManifPose( + kinDyn->getWorldTransform(kinDynWrapperList[0]->getFloatingBase())); + + // Set the sub-model state + kinDynWrapperList[0]->setRobotState(worldTBase.transform(), + jointPos, + iDynTree::make_span(baseVel.data(), + manif::SE3d::Tangent::DoF), + jointVel, + gravity); + + // Forward dynamics + Eigen::VectorXd jointAccFD = Eigen::VectorXd(numJoints); + REQUIRE(kinDynWrapperList[0]->forwardDynamics(jointTrq, + Eigen::VectorXd::Zero(numJoints), + Eigen::VectorXd::Zero(numJoints), + baseAcc, + jointAccFD)); + + constexpr double tolerance = 1e-2; + REQUIRE(jointAcc.isApprox(jointAccFD, tolerance)); + + // Compute wrench at the base + Eigen::VectorXd trqExt = Eigen::VectorXd::Zero(6 + numJoints); + Eigen::VectorXd genTrq = Eigen::VectorXd::Zero(6 + numJoints); + REQUIRE(kinDynWrapperList[0]->generalizedBiasForces(genTrq)); + Eigen::MatrixXd massMatrix = Eigen::MatrixXd::Zero(6 + numJoints, 6 + numJoints); + REQUIRE(kinDynWrapperList[0]->getFreeFloatingMassMatrix(massMatrix)); + + Eigen::VectorXd tempVec = massMatrix.topLeftCorner(6, 6) * baseAcc.coeffs(); + tempVec += massMatrix.topRightCorner(6, numJoints) * jointAcc; + trqExt.head(6) = tempVec + genTrq.head(6); + + Eigen::VectorXd nuDot = Eigen::VectorXd(6 + numJoints); + REQUIRE(kinDynWrapperList[0]->forwardDynamics(jointTrq, + Eigen::VectorXd::Zero(numJoints), + trqExt, + nuDot)); + + REQUIRE(nuDot.head(6).isApprox(baseAcc.coeffs(), tolerance)); + REQUIRE(nuDot.tail(numJoints).isApprox(jointAcc, tolerance)); +}