diff --git a/CHANGELOG.md b/CHANGELOG.md index 0173d6f7e1..7dd85d0f3b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -3,6 +3,8 @@ All notable changes to this project are documented in this file. ## [unreleased] ### Added +- Set submodel states from IMUs in RDE and add friction torques as measurement (https://github.com/ami-iit/bipedal-locomotion-framework/pull/793) + ### Changed ### Fixed ### Removed @@ -552,4 +554,4 @@ All notable changes to this project are documented in this file. [0.3.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.2.0...v0.3.0 [0.2.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.1.1...v0.2.0 [0.1.1]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.1.0...v0.1.1 -[0.1.0]: https://github.com/ami-iit/bipedal-locomotion-framework/releases/tag/v0.1.0 +[0.1.0]: https://github.com/ami-iit/bipedal-locomotion-framework/releases/tag/v0.1.0 \ No newline at end of file diff --git a/bindings/python/RobotDynamicsEstimator/src/RobotDynamicsEstimator.cpp b/bindings/python/RobotDynamicsEstimator/src/RobotDynamicsEstimator.cpp index 4b5ca7fcfe..23caee3efb 100644 --- a/bindings/python/RobotDynamicsEstimator/src/RobotDynamicsEstimator.cpp +++ b/bindings/python/RobotDynamicsEstimator/src/RobotDynamicsEstimator.cpp @@ -49,7 +49,7 @@ void CreateRobotDynamicsEstimator(pybind11::module& module) .def_readwrite("ft_wrenches", &RobotDynamicsEstimatorInput::ftWrenches) .def_readwrite("linear_accelerations", &RobotDynamicsEstimatorInput::linearAccelerations) .def_readwrite("angular_velocities", &RobotDynamicsEstimatorInput::angularVelocities) - .def_readwrite("friction", &RobotDynamicsEstimatorInput::friction); + .def_readwrite("friction_torques", &RobotDynamicsEstimatorInput::frictionTorques); py::class_(module, "RobotDynamicsEstimatorOutput") .def(py::init()) @@ -58,7 +58,9 @@ void CreateRobotDynamicsEstimator(pybind11::module& module) .def_readwrite("tau_F", &RobotDynamicsEstimatorOutput::tau_F) .def_readwrite("ft_wrenches", &RobotDynamicsEstimatorOutput::ftWrenches) .def_readwrite("ft_wrenches_biases", &RobotDynamicsEstimatorOutput::ftWrenchesBiases) + .def_readwrite("linear_accelerations", &RobotDynamicsEstimatorOutput::linearAccelerations) .def_readwrite("accelerometer_biases", &RobotDynamicsEstimatorOutput::accelerometerBiases) + .def_readwrite("angular_velocities", &RobotDynamicsEstimatorOutput::angularVelocities) .def_readwrite("gyroscope_biases", &RobotDynamicsEstimatorOutput::gyroscopeBiases) .def_readwrite("contact_wrenches", &RobotDynamicsEstimatorOutput::contactWrenches) .def(py::pickle([](const RobotDynamicsEstimatorOutput &output) { //__getstate__ @@ -67,7 +69,9 @@ void CreateRobotDynamicsEstimator(pybind11::module& module) output.tau_F, output.ftWrenches, output.ftWrenchesBiases, + output.linearAccelerations, output.accelerometerBiases, + output.angularVelocities, output.gyroscopeBiases, output.contactWrenches); }, @@ -80,9 +84,11 @@ void CreateRobotDynamicsEstimator(pybind11::module& module) rde.tau_F = t[2].cast(); rde.ftWrenches = t[3].cast>(); rde.ftWrenchesBiases = t[4].cast>(); - rde.accelerometerBiases = t[5].cast>(); - rde.gyroscopeBiases = t[6].cast>(); - rde.contactWrenches = t[7].cast>(); + rde.linearAccelerations = t[5].cast>(); + rde.accelerometerBiases = t[6].cast>(); + rde.angularVelocities = t[7].cast>(); + rde.gyroscopeBiases = t[8].cast>(); + rde.contactWrenches = t[9].cast>(); return rde; } diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h index 1f67c55197..00bfb5f5c8 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h @@ -82,17 +82,18 @@ class AccelerometerMeasurementDynamics : public Dynamics /** * Initialize the state dynamics. * @param paramHandler pointer to the parameters handler. + * @param name name of the dynamics. * @note the following parameters are required by the class * | Parameter Name | Type | Description | Mandatory | * |:----------------------------------:|:--------:|:-------------------------------------------------------------------------------------------------------:|:---------:| - * | `name` | `string` | Name of the state contained in the `VariablesHandler` describing the state associated to this dynamics| Yes | + * | `input_name` | `string` | Name of the variable in input to the main application corresponding to this dynamics | Yes | * | `covariance` | `vector` | Process covariances | Yes | * | `dynamic_model` | `string` | Type of dynamic model describing the state dynamics. | Yes | * | `use_bias` |`boolean` | Boolean saying if the dynamics depends on a bias. False if not specified. | No | * @return True in case of success, false otherwise. */ bool - initialize(std::weak_ptr paramHandler) override; + initialize(std::weak_ptr paramHandler, const std::string& name) override; /** * Finalize the Dynamics. diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ConstantMeasurementModel.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ConstantMeasurementModel.h index 6463707aac..4ac4be92aa 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ConstantMeasurementModel.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ConstantMeasurementModel.h @@ -50,17 +50,18 @@ class ConstantMeasurementModel : public Dynamics /** * Initialize the state dynamics. * @param paramHandler pointer to the parameters handler. + * @param name name of the dynamics. * @note the following parameters are required by the class * | Parameter Name | Type | Description | Mandatory | * |:----------------------------------:|:--------:|:-------------------------------------------------------------------------------------------------------:|:---------:| - * | `name` | `string` | Name of the state contained in the `VariablesHandler` describing the state associated to this dynamics| Yes | - * | `covariance` | `vector` | Process covariances | Yes | + * | `input_name` | `string` | Name of the variable in input to the main application corresponding to this dynamics. | Yes | + * | `associated_state` | `string` | Name of the variable in the state vector corresponding to this dynamics. | Yes | + * | `covariance` | `vector` | Process covariances. | Yes | * | `dynamic_model` | `string` | Type of dynamic model describing the state dynamics. | Yes | - * | `elements` | `vector` | Vector of strings describing the list of sub variables composing the state associated to this dynamics.| No | * | `use_bias` |`boolean` | Boolean saying if the dynamics depends on a bias. False if not specified. | No | * @return True in case of success, false otherwise. */ - bool initialize(std::weak_ptr paramHandler) override; + bool initialize(std::weak_ptr paramHandler, const std::string& name) override; /** * Finalize the Dynamics. diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h index 77265a4c24..cfaab45076 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h @@ -118,10 +118,11 @@ class Dynamics /** * Initialize the task. * @param paramHandler pointer to the parameters handler. + * @param name name of the dynamics. * @return True in case of success, false otherwise. */ virtual bool - initialize(std::weak_ptr paramHandler); + initialize(std::weak_ptr paramHandler, const std::string& name); /** * Finalize the Dynamics. diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ExternalContactStateDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ExternalContactStateDynamics.h index 2a7b1a929c..b88cd8aef2 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ExternalContactStateDynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ExternalContactStateDynamics.h @@ -51,17 +51,17 @@ class ExternalContactStateDynamics : public Dynamics /** * Initialize the state dynamics. * @param paramHandler pointer to the parameters handler. + * @param name name of the dynamics. * @note the following parameters are required by the class * | Parameter Name | Type | Description | Mandatory | * |:----------------------------------:|:--------:|:-------------------------------------------------------------------------------------------------------:|:---------:| - * | `name` | `string` | Name of the state contained in the `VariablesHandler` describing the state associated to this dynamics| Yes | + * | `input_name` | `string` | Name of the variable in input to the main application corresponding to this dynamics | Yes | * | `covariance` | `vector` | Process covariances | Yes | * | `initial_covariance` | `vector` | Initial state covariances | Yes | * | `dynamic_model` | `string` | Type of dynamic model describing the state dynamics. | Yes | - * | `elements` | `vector` | Vector of strings describing the list of sub variables composing the state associated to this dynamics.| No | * @return True in case of success, false otherwise. */ - bool initialize(std::weak_ptr paramHandler) override; + bool initialize(std::weak_ptr paramHandler, const std::string& name) override; /** * Finalize the Dynamics. diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h index 67545cc548..7d958ac504 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h @@ -64,14 +64,14 @@ class FrictionTorqueStateDynamics : public Dynamics /** * Initialize the state dynamics. * @param paramHandler pointer to the parameters handler. + * @param name name of the dynamics. * @note the following parameters are required by the class * | Parameter Name | Type | Description | Mandatory | * |:----------------------------------:|:--------:|:-------------------------------------------------------------------------------------------------------:|:---------:| - * | `name` | `string` | Name of the state contained in the `VariablesHandler` describing the state associated to this dynamics| Yes | + * | `input_name` | `string` | Name of the variable in input to the main application corresponding to this dynamics | Yes | * | `covariance` | `vector` | Process covariances | Yes | * | `initial_covariance` | `vector` | Initial state covariances | Yes | * | `dynamic_model` | `string` | Type of dynamic model describing the state dynamics. | Yes | - * | `elements` | `vector` | Vector of strings describing the list of sub variables composing the state associated to this dynamics.| No | * | `friction_k0` | `vector` | Vector of coefficients k0. For more info check the class description. | Yes | * | `friction_k1` | `vector` | Vector of coefficients k1. For more info check the class description. | Yes | * | `friction_k2` | `vector` | Vector of coefficients k2. For more info check the class description. | Yes | @@ -79,7 +79,7 @@ class FrictionTorqueStateDynamics : public Dynamics * @return True in case of success, false otherwise. */ bool - initialize(std::weak_ptr paramHandler) override; + initialize(std::weak_ptr paramHandler, const std::string& name) override; /** * Finalize the Dynamics. diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/GyroscopeMeasurementDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/GyroscopeMeasurementDynamics.h index e9e33ebf72..024bb3c213 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/GyroscopeMeasurementDynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/GyroscopeMeasurementDynamics.h @@ -69,17 +69,18 @@ class GyroscopeMeasurementDynamics : public Dynamics /** * Initialize the state dynamics. * @param paramHandler pointer to the parameters handler. + * @param name name of the dynamics. * @note the following parameters are required by the class * | Parameter Name | Type | Description | Mandatory | * |:----------------------------------:|:--------:|:-------------------------------------------------------------------------------------------------------:|:---------:| - * | `name` | `string` | Name of the state contained in the `VariablesHandler` describing the state associated to this dynamics| Yes | + * | `input_name` | `string` | Name of the variable in input to the main application corresponding to this dynamics | Yes | * | `covariance` | `vector` | Process covariances | Yes | * | `dynamic_model` | `string` | Type of dynamic model describing the state dynamics. | Yes | * | `use_bias` |`boolean` | Boolean saying if the dynamics depends on a bias. False if not specified. | No | * @return True in case of success, false otherwise. */ bool - initialize(std::weak_ptr paramHandler) override; + initialize(std::weak_ptr paramHandler, const std::string& name) override; /** * Finalize the Dynamics. diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h index ea57690f9f..14a37fe0f1 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h @@ -69,18 +69,18 @@ class JointVelocityStateDynamics : public Dynamics /** * Initialize the state dynamics. * @param paramHandler pointer to the parameters handler. + * @param name name of the dynamics. * @note the following parameters are required by the class * | Parameter Name | Type | Description | Mandatory | * |:----------------------------------:|:---------------------:|:-------------------------------------------------------------------------------------------------------:|:---------:| - * | `name` | `string` | Name of the state contained in the `VariablesHandler` describing the state associated to this dynamics| Yes | + * | `input_name` | `string` | Name of the variable in input to the main application corresponding to this dynamics | Yes | * | `covariance` | `vector` | Process covariances | Yes | * | `initial_covariance` | `vector` | Initial state covariances | Yes | * | `dynamic_model` | `string` | Type of dynamic model describing the state dynamics. | Yes | - * | `elements` | `vector` | Vector of strings describing the list of sub variables composing the state associated to this dynamics.| No | * | `dT` | `chrono::nanoseconds` | Sampling time. | Yes | * @return True in case of success, false otherwise. */ - bool initialize(std::weak_ptr paramHandler) override; + bool initialize(std::weak_ptr paramHandler, const std::string& name) override; /** * Finalize the Dynamics. diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/MotorCurrentMeasurementDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/MotorCurrentMeasurementDynamics.h index 67d23438ae..f8633721c5 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/MotorCurrentMeasurementDynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/MotorCurrentMeasurementDynamics.h @@ -56,19 +56,19 @@ class MotorCurrentMeasurementDynamics : public Dynamics /** * Initialize the measurement object. * @param paramHandler pointer to the parameters handler. + * @param name name of the dynamics. * @note the following parameters are required by the class * | Parameter Name | Type | Description | Mandatory | * |:----------------------------------:|:--------:|:-------------------------------------------------------------------------------------------------------:|:---------:| - * | `name` | `string` |Name of the measurement variable contained in the `VariablesHandler` describing this dynamics | Yes | + * | `input_name` | `string` | Name of the variable in input to the main application corresponding to this dynamics | Yes | * | `covariance` | `vector` | Process covariances | Yes | * | `dynamic_model` | `string` | Type of dynamic model describing the measurement dynamics. | Yes | - * | `elements` | `vector` |Vector of strings describing the list of variables composing the measurement dynamics. | No | * | `torque_constant` | `vector` | Vector of coefficients k0. For more info check the class description. | Yes | * | `gear_ratio` | `vector` | Vector of coefficients k1. For more info check the class description. | Yes | * @return True in case of success, false otherwise. */ bool - initialize(std::weak_ptr paramHandler) override; + initialize(std::weak_ptr paramHandler, const std::string& name) override; /** * Finalize the Dynamics. diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/RobotDynamicsEstimator.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/RobotDynamicsEstimator.h index ef905d4b34..832d06cd03 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/RobotDynamicsEstimator.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/RobotDynamicsEstimator.h @@ -41,7 +41,7 @@ struct RobotDynamicsEstimatorInput 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. */ + Eigen::VectorXd frictionTorques; /**< Friction torques in Nm. */ }; /** @@ -58,6 +58,8 @@ struct RobotDynamicsEstimatorOutput std::map accelerometerBiases; /**< Biases of the accelerometer sensors. */ std::map gyroscopeBiases; /**< Biases of the gyroscope sensors. */ std::map contactWrenches; /**< External contact wrenches. */ + std::map linearAccelerations; /**< Linear acceleration of the accelerometer frames. */ + std::map angularVelocities; /**< Angular velocity of the gyroscope frames. */ }; /** diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h index 6dd3fe19d3..6686e081eb 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h @@ -35,7 +35,6 @@ struct Sensor { std::string name; /**< Name of the sensor in the URDF */ std::string frame; /**< Frame of the sensor in the URDF */ - std::string ukfName; /**< Name of the dynamics used for this sensor. */ int frameIndex; /**< Index of the sensor in the URDF. */ }; @@ -77,6 +76,8 @@ class SubModel std::unordered_map m_externalContactList; /**< List of the additional external contacts */ Sensor dummySensor{}; /**< Dummy sensor. */ FTSensor dummyFT{}; /**< Dummy FT sensor. */ + int m_imuBaseFrameIndex; /**< Name of the IMU frame rigidly attached to the sub-model base frame. */ + std::string m_imuBaseFrameName; /**< Name of the IMU frame rigidly attached to the sub-model base frame. */ public: /** @@ -200,6 +201,18 @@ class SubModel */ const Sensor& getExternalContactIndex(const std::string& name); + /** + * @brief getImuBaseFrameName get the name of the IMU frame rigidly attached to the sub-model base frame. + * @return a string containing the name of the IMU frame. + */ + const std::string getImuBaseFrameName() const; + + /** + * @brief getImuBaseFrameIndex get the index of the IMU frame rigidly attached to the sub-model base frame. + * @return an integer containing the index of the IMU frame. + */ + int getImuBaseFrameIndex() const; + friend class SubModelCreator; }; diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfMeasurement.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfMeasurement.h index b80e8694d1..d7753021ec 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfMeasurement.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfMeasurement.h @@ -229,6 +229,13 @@ class UkfMeasurement : public bfl::AdditiveMeasurementModel, UkfModel */ bool freeze(const bfl::Data& data = bfl::Data()) override; + /** + * @brief setMeasurementNameMapping set the mapping between the name of the measurement and the name of the variable in the ukf. + * @param measurementToUkfNames is a map containing the mapping between the name of the measurement and the name of the variable in the ukf. + * @note the map should contain only the name of the state dynamics. + */ + void setStateNameMapping(const std::map& stateToUkfNames); + }; // classUkfMeasurement } // namespace RobotDynamicsEstimator diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfModel.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfModel.h index 5efcb25c80..e9e88aba0e 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfModel.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfModel.h @@ -75,6 +75,10 @@ class UkfModel values of the wrench */ std::map m_extContactMap; /**< The map contains names of the ft sensors and values of the wrench */ + std::map m_accMap; /**< The map contains names of the accelerometer sensors and + values of the linear accelerations */ + std::map m_gyroMap; /**< The map contains names of the gyroscope sensors and + values of the angular velocities */ 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. */ @@ -85,6 +89,13 @@ class UkfModel 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. */ + std::map m_stateToUkfNames; /**< Map used to retrieve the name of the variable passed as state and the ukf name. */ + std::map m_ukfNamesToMeasures; /**< Map used to retrieve the name of the variable passed as input and the ukf name. */ + Eigen::Vector3d m_sensorLinearAcceleration; /**< Linear acceleration measured by an accelerometer. */ + Eigen::Vector3d m_bOmegaIB; /**< Angular velocity of a frame. */ + manif::SE3Tangentd m_tempAccelerometerVelocity; /**< Velocity of an accelerometer. */ + manif::SE3Tangentd m_baseVelocity; /**< Submodel base velocity. */ + manif::SE3Tangentd m_baseAcceleration; /**< Submodel base acceleration. */ public: /** diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h index 53df175c0c..d120d664be 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h @@ -183,7 +183,6 @@ class UkfState : public bfl::AdditiveStateModel, UkfModel * @return a Eigen reference to the Eigen Matrix covariance. */ Eigen::Ref getInitialStateCovarianceMatrix() const; - }; // class UKFModel } // namespace RobotDynamicsEstimator diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityStateDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityStateDynamics.h index 020349e465..3a80a72263 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityStateDynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityStateDynamics.h @@ -47,17 +47,17 @@ class ZeroVelocityStateDynamics : public Dynamics /** * Initialize the state dynamics. * @param paramHandler pointer to the parameters handler. + * @param name name of the dynamics. * @note the following parameters are required by the class * | Parameter Name | Type | Description | Mandatory | * |:----------------------------------:|:--------:|:-------------------------------------------------------------------------------------------------------:|:---------:| - * | `name` | `string` | Name of the state contained in the `VariablesHandler` describing the state associated to this dynamics| Yes | + * | `input_name` | `string` | Name of the variable in input to the main application corresponding to this dynamics | Yes | * | `covariance` | `vector` | Process covariances | Yes | * | `initial_covariance` | `vector` | Initial state covariances | Yes | * | `dynamic_model` | `string` | Type of dynamic model describing the state dynamics. | Yes | - * | `elements` | `vector` | Vector of strings describing the list of sub variables composing the state associated to this dynamics.| No | * @return True in case of success, false otherwise. */ - bool initialize(std::weak_ptr paramHandler) override; + bool initialize(std::weak_ptr paramHandler, const std::string& name) override; /** * Finalize the Dynamics. diff --git a/src/Estimators/src/AccelerometerMeasurementDynamics.cpp b/src/Estimators/src/AccelerometerMeasurementDynamics.cpp index 038ef6f249..88bf450e50 100644 --- a/src/Estimators/src/AccelerometerMeasurementDynamics.cpp +++ b/src/Estimators/src/AccelerometerMeasurementDynamics.cpp @@ -19,7 +19,8 @@ RDE::AccelerometerMeasurementDynamics::AccelerometerMeasurementDynamics() = defa RDE::AccelerometerMeasurementDynamics::~AccelerometerMeasurementDynamics() = default; bool RDE::AccelerometerMeasurementDynamics::initialize( - std::weak_ptr paramHandler) + std::weak_ptr paramHandler, + const std::string& name) { constexpr auto errorPrefix = "[AccelerometerMeasurementDynamics::initialize]"; @@ -30,12 +31,7 @@ bool RDE::AccelerometerMeasurementDynamics::initialize( return false; } - // Set the state dynamics name - if (!ptr->getParameter("name", m_name)) - { - log()->error("{} Error while retrieving the name variable.", errorPrefix); - return false; - } + m_name = name; // Set the state process covariance if (!ptr->getParameter("covariance", m_covSingleVar)) diff --git a/src/Estimators/src/ConstantMeasurementModel.cpp b/src/Estimators/src/ConstantMeasurementModel.cpp index 8a41a65efb..25a32a104b 100644 --- a/src/Estimators/src/ConstantMeasurementModel.cpp +++ b/src/Estimators/src/ConstantMeasurementModel.cpp @@ -5,12 +5,14 @@ * distributed under the terms of the BSD-3-Clause license. */ -#include #include +#include namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; -bool RDE::ConstantMeasurementModel::initialize(std::weak_ptr paramHandler) +bool RDE::ConstantMeasurementModel::initialize( + std::weak_ptr paramHandler, + const std::string& name) { constexpr auto errorPrefix = "[ConstantMeasurementModel::initialize]"; @@ -21,10 +23,9 @@ bool RDE::ConstantMeasurementModel::initialize(std::weak_ptrgetParameter("name", m_name)) + if (!ptr->getParameter("associated_state", m_name)) { - log()->error("{} Error while retrieving the name variable.", errorPrefix); + log()->error("{} Error while retrieving the associated state variable.", errorPrefix); return false; } @@ -45,8 +46,7 @@ bool RDE::ConstantMeasurementModel::initialize(std::weak_ptrgetParameter("use_bias", m_useBias)) { log()->debug("{} Variable use_bias not found. Set to false by default.", errorPrefix); - } - else + } else { m_biasVariableName = m_name + "_bias"; } @@ -58,7 +58,7 @@ bool RDE::ConstantMeasurementModel::initialize(std::weak_ptr& /*subModelList*/, const std::vector>& /*kinDynWrapperList*/) +bool RDE::ConstantMeasurementModel::setSubModels( + const std::vector& /*subModelList*/, + const std::vector>& /*kinDynWrapperList*/) { return true; } @@ -114,7 +116,9 @@ bool RDE::ConstantMeasurementModel::checkStateVariableHandler() { if (!m_stateVariableHandler.getVariable(m_biasVariableName).isValid()) { - log()->error("{} The variable handler does not contain the expected state name {}.", errorPrefix, m_biasVariableName); + log()->error("{} The variable handler does not contain the expected state name {}.", + errorPrefix, + m_biasVariableName); return false; } } @@ -127,8 +131,7 @@ bool RDE::ConstantMeasurementModel::update() if (m_useBias) { m_updatedVariable = m_currentState + m_bias; - } - else + } else { m_updatedVariable = m_currentState; } diff --git a/src/Estimators/src/Dynamics.cpp b/src/Estimators/src/Dynamics.cpp index e0e5aad3d8..0fbf4e95ad 100644 --- a/src/Estimators/src/Dynamics.cpp +++ b/src/Estimators/src/Dynamics.cpp @@ -33,7 +33,8 @@ bool UkfInputProvider::isOutputValid() const return m_ukfInput.robotJointPositions.size() != 0; } -bool Dynamics::initialize(std::weak_ptr /**paramHandler**/) +bool Dynamics::initialize(std::weak_ptr /**paramHandler**/, + const std::string& /**name**/) { return true; } diff --git a/src/Estimators/src/ExternalContactStateDynamics.cpp b/src/Estimators/src/ExternalContactStateDynamics.cpp index 57be2b9a18..0db317fe84 100644 --- a/src/Estimators/src/ExternalContactStateDynamics.cpp +++ b/src/Estimators/src/ExternalContactStateDynamics.cpp @@ -11,7 +11,8 @@ namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; bool RDE::ExternalContactStateDynamics::initialize( - std::weak_ptr paramHandler) + std::weak_ptr paramHandler, + const std::string& name) { constexpr auto errorPrefix = "[ExternalContactStateDynamics::initialize]"; @@ -22,12 +23,7 @@ bool RDE::ExternalContactStateDynamics::initialize( return false; } - // Set the state dynamics name - if (!ptr->getParameter("name", m_name)) - { - log()->error("{} Error while retrieving the name variable.", errorPrefix); - return false; - } + m_name = name; // Set the state process covariance if (!ptr->getParameter("covariance", m_covariances)) diff --git a/src/Estimators/src/FrictionTorqueStateDynamics.cpp b/src/Estimators/src/FrictionTorqueStateDynamics.cpp index 66e2fc9d1d..0926e4c6d0 100644 --- a/src/Estimators/src/FrictionTorqueStateDynamics.cpp +++ b/src/Estimators/src/FrictionTorqueStateDynamics.cpp @@ -19,7 +19,8 @@ RDE::FrictionTorqueStateDynamics::FrictionTorqueStateDynamics() = default; RDE::FrictionTorqueStateDynamics::~FrictionTorqueStateDynamics() = default; bool RDE::FrictionTorqueStateDynamics::initialize( - std::weak_ptr paramHandler) + std::weak_ptr paramHandler, + const std::string& name) { constexpr auto errorPrefix = "[FrictionTorqueStateDynamics::initialize]"; @@ -30,12 +31,7 @@ bool RDE::FrictionTorqueStateDynamics::initialize( return false; } - // Set the state dynamics name - if (!ptr->getParameter("name", m_name)) - { - log()->error("{} Error while retrieving the name variable.", errorPrefix); - return false; - } + m_name = name; // Set the state process covariance if (!ptr->getParameter("covariance", m_covariances)) @@ -115,10 +111,10 @@ bool RDE::FrictionTorqueStateDynamics::finalize(const System::VariablesHandler& m_size = m_covariances.size(); - m_frictionTorqueFullModel.resize(m_stateVariableHandler.getVariable("tau_F").size); + m_frictionTorqueFullModel.resize(m_stateVariableHandler.getVariable("FRICTION_TORQUES").size); m_frictionTorqueFullModel.setZero(); - m_jointVelocityFullModel.resize(m_stateVariableHandler.getVariable("ds").size); + m_jointVelocityFullModel.resize(m_stateVariableHandler.getVariable("JOINT_VELOCITIES").size); m_jointVelocityFullModel.setZero(); m_updatedVariable.resize(m_size); @@ -151,17 +147,17 @@ bool RDE::FrictionTorqueStateDynamics::checkStateVariableHandler() constexpr auto errorPrefix = "[FrictionTorqueStateDynamics::checkStateVariableHandler]"; // Check if the variable handler contains the variables used by this dynamics - if (!m_stateVariableHandler.getVariable("tau_F").isValid()) + if (!m_stateVariableHandler.getVariable("FRICTION_TORQUES").isValid()) { log()->error("{} The variable handler does not contain the expected state with name " - "`tau_F`.", + "`FRICTION_TORQUES`.", errorPrefix); return false; } - if (!m_stateVariableHandler.getVariable("ds").isValid()) + if (!m_stateVariableHandler.getVariable("JOINT_VELOCITIES").isValid()) { - log()->error("{} The variable handler does not contain the expected state with name `ds`.", + log()->error("{} The variable handler does not contain the expected state with name `JOINT_VELOCITIES`.", errorPrefix); return false; } @@ -191,11 +187,11 @@ bool RDE::FrictionTorqueStateDynamics::update() void RDE::FrictionTorqueStateDynamics::setState(const Eigen::Ref ukfState) { - m_jointVelocityFullModel = ukfState.segment(m_stateVariableHandler.getVariable("ds").offset, - m_stateVariableHandler.getVariable("ds").size); + m_jointVelocityFullModel = ukfState.segment(m_stateVariableHandler.getVariable("JOINT_VELOCITIES").offset, + m_stateVariableHandler.getVariable("JOINT_VELOCITIES").size); - m_frictionTorqueFullModel = ukfState.segment(m_stateVariableHandler.getVariable("tau_F").offset, - m_stateVariableHandler.getVariable("tau_F").size); + m_frictionTorqueFullModel = ukfState.segment(m_stateVariableHandler.getVariable("FRICTION_TORQUES").offset, + m_stateVariableHandler.getVariable("FRICTION_TORQUES").size); } void RDE::FrictionTorqueStateDynamics::setInput(const UKFInput& ukfInput) diff --git a/src/Estimators/src/GyroscopeMeasurementDynamics.cpp b/src/Estimators/src/GyroscopeMeasurementDynamics.cpp index 9713b5e3b8..327b28697c 100644 --- a/src/Estimators/src/GyroscopeMeasurementDynamics.cpp +++ b/src/Estimators/src/GyroscopeMeasurementDynamics.cpp @@ -18,7 +18,8 @@ RDE::GyroscopeMeasurementDynamics::GyroscopeMeasurementDynamics() = default; RDE::GyroscopeMeasurementDynamics::~GyroscopeMeasurementDynamics() = default; bool RDE::GyroscopeMeasurementDynamics::initialize( - std::weak_ptr paramHandler) + std::weak_ptr paramHandler, + const std::string& name) { constexpr auto errorPrefix = "[GyroscopeMeasurementDynamics::initialize]"; @@ -29,12 +30,7 @@ bool RDE::GyroscopeMeasurementDynamics::initialize( return false; } - // Set the state dynamics name - if (!ptr->getParameter("name", m_name)) - { - log()->error("{} Error while retrieving the name variable.", errorPrefix); - return false; - } + m_name = name; // Set the state process covariance if (!ptr->getParameter("covariance", m_covSingleVar)) @@ -102,7 +98,7 @@ bool RDE::GyroscopeMeasurementDynamics::finalize( m_size = m_covariances.size(); - m_jointVelocityFullModel.resize(m_stateVariableHandler.getVariable("ds").size); + m_jointVelocityFullModel.resize(m_stateVariableHandler.getVariable("JOINT_VELOCITIES").size); m_jointVelocityFullModel.setZero(); m_subModelJointVel.resize(m_nrOfSubDynamics); @@ -149,9 +145,9 @@ bool RDE::GyroscopeMeasurementDynamics::checkStateVariableHandler() { constexpr auto errorPrefix = "[GyroscopeMeasurementDynamics::checkStateVariableHandler]"; - if (!m_stateVariableHandler.getVariable("ds").isValid()) + if (!m_stateVariableHandler.getVariable("JOINT_VELOCITIES").isValid()) { - log()->error("{} The variable handler does not contain the expected state with name `ds`.", + log()->error("{} The variable handler does not contain the expected state with name `JOINT_VELOCITIES`.", errorPrefix); return false; } @@ -194,8 +190,8 @@ bool RDE::GyroscopeMeasurementDynamics::update() void RDE::GyroscopeMeasurementDynamics::setState(const Eigen::Ref ukfState) { - m_jointVelocityFullModel = ukfState.segment(m_stateVariableHandler.getVariable("ds").offset, - m_stateVariableHandler.getVariable("ds").size); + m_jointVelocityFullModel = ukfState.segment(m_stateVariableHandler.getVariable("JOINT_VELOCITIES").offset, + m_stateVariableHandler.getVariable("JOINT_VELOCITIES").size); for (int smIndex = 0; smIndex < m_subModelList.size(); smIndex++) { @@ -216,5 +212,5 @@ void RDE::GyroscopeMeasurementDynamics::setState(const Eigen::Ref -#include #include +#include +#include namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; @@ -15,7 +15,9 @@ RDE::JointVelocityStateDynamics::JointVelocityStateDynamics() = default; RDE::JointVelocityStateDynamics::~JointVelocityStateDynamics() = default; -bool RDE::JointVelocityStateDynamics::setSubModels(const std::vector& /*subModelList*/, const std::vector>& /*kinDynWrapperList*/) +bool RDE::JointVelocityStateDynamics::setSubModels( + const std::vector& /*subModelList*/, + const std::vector>& /*kinDynWrapperList*/) { return true; } @@ -25,7 +27,9 @@ bool RDE::JointVelocityStateDynamics::checkStateVariableHandler() return true; } -bool RDE::JointVelocityStateDynamics::initialize(std::weak_ptr paramHandler) +bool RDE::JointVelocityStateDynamics::initialize( + std::weak_ptr paramHandler, + const std::string& name) { constexpr auto errorPrefix = "[JointVelocityStateDynamics::initialize]"; @@ -36,12 +40,7 @@ bool RDE::JointVelocityStateDynamics::initialize(std::weak_ptrgetParameter("name", m_name)) - { - log()->error("{} Error while retrieving the name variable.", errorPrefix); - return false; - } + m_name = name; // Set the state process covariance if (!ptr->getParameter("covariance", m_covariances)) @@ -60,7 +59,7 @@ bool RDE::JointVelocityStateDynamics::initialize(std::weak_ptrgetParameter("elements", m_elements)) { - log()->info("{} Variable elements not found.", errorPrefix); + log()->debug("{} Variable elements not found.", errorPrefix); } if (!ptr->getParameter("sampling_time", m_dT)) @@ -76,7 +75,7 @@ bool RDE::JointVelocityStateDynamics::initialize(std::weak_ptr(m_dT).count() * m_ukfInput.robotJointAccelerations; + m_updatedVariable + = m_jointVelocityFullModel + + std::chrono::duration(m_dT).count() * m_ukfInput.robotJointAccelerations; return true; } void RDE::JointVelocityStateDynamics::setState(const Eigen::Ref ukfState) { - m_jointVelocityFullModel = ukfState.segment(m_stateVariableHandler.getVariable("ds").offset, - m_stateVariableHandler.getVariable("ds").size); + m_jointVelocityFullModel = ukfState.segment(m_stateVariableHandler.getVariable(m_name).offset, + m_stateVariableHandler.getVariable(m_name).size); } void RDE::JointVelocityStateDynamics::setInput(const UKFInput& ukfInput) diff --git a/src/Estimators/src/MotorCurrentMeasurementDynamics.cpp b/src/Estimators/src/MotorCurrentMeasurementDynamics.cpp index f88f9d46a4..2a4e063760 100644 --- a/src/Estimators/src/MotorCurrentMeasurementDynamics.cpp +++ b/src/Estimators/src/MotorCurrentMeasurementDynamics.cpp @@ -17,7 +17,8 @@ RDE::MotorCurrentMeasurementDynamics::MotorCurrentMeasurementDynamics() = defaul RDE::MotorCurrentMeasurementDynamics::~MotorCurrentMeasurementDynamics() = default; bool RDE::MotorCurrentMeasurementDynamics::initialize( - std::weak_ptr paramHandler) + std::weak_ptr paramHandler, + const std::string& name) { constexpr auto errorPrefix = "[MotorCurrentMeasurementDynamics::initialize]"; @@ -28,12 +29,7 @@ bool RDE::MotorCurrentMeasurementDynamics::initialize( return false; } - // Set the state dynamics name - if (!ptr->getParameter("name", m_name)) - { - log()->error("{} Error while retrieving the name variable.", errorPrefix); - return false; - } + m_name = name; // Set the state process covariance if (!ptr->getParameter("covariance", this->m_covariances)) @@ -45,7 +41,7 @@ bool RDE::MotorCurrentMeasurementDynamics::initialize( // Set the list of elements if it exists if (!ptr->getParameter("elements", m_elements)) { - log()->info("{} Variable elements not found.", errorPrefix); + log()->debug("{} Variable elements not found.", errorPrefix); } // Set the torque constants @@ -117,11 +113,11 @@ bool RDE::MotorCurrentMeasurementDynamics::checkStateVariableHandler() constexpr auto errorPrefix = "[MotorCurrentMeasurementDynamics::checkStateVariableHandler]"; // Check if the variable handler contains the variables used by this dynamics - if (!m_stateVariableHandler.getVariable("tau_m").isValid()) + if (!m_stateVariableHandler.getVariable("MOTOR_TORQUES").isValid()) { log()->error("{} The variable handler does not contain the expected measurement name {}.", errorPrefix, - "tau_m"); + "MOTOR_TORQUES"); return false; } @@ -137,8 +133,8 @@ bool RDE::MotorCurrentMeasurementDynamics::update() void RDE::MotorCurrentMeasurementDynamics::setState(const Eigen::Ref ukfState) { - m_motorTorque = ukfState.segment(m_stateVariableHandler.getVariable("tau_m").offset, - m_stateVariableHandler.getVariable("tau_m").size); + m_motorTorque = ukfState.segment(m_stateVariableHandler.getVariable("MOTOR_TORQUES").offset, + m_stateVariableHandler.getVariable("MOTOR_TORQUES").size); } void RDE::MotorCurrentMeasurementDynamics::setInput(const UKFInput& /*ukfInput*/) diff --git a/src/Estimators/src/RobotDynamicsEstimator.cpp b/src/Estimators/src/RobotDynamicsEstimator.cpp index 69dccfc53e..79aa690866 100644 --- a/src/Estimators/src/RobotDynamicsEstimator.cpp +++ b/src/Estimators/src/RobotDynamicsEstimator.cpp @@ -5,17 +5,17 @@ * 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; @@ -25,10 +25,20 @@ 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::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. */ + std::map ukfMeasurementFromSensors; /**< This map containes the + measurement coming from the + sensors and needed for the + correction phase of the + estimation. */ + + std::map inputNameToUkfState; /**< Map used to retrieve the state name + from the input name. */ + std::map> inputNameToUkfMeasurement; /**< Map used to + retrieve the measurement name + from the input name. */ bfl::Gaussian predictedState; /**< Predicted state computed by the `predict` method. */ bfl::Gaussian correctedState; /**< Corrected state computed by the `correct` method. */ @@ -62,7 +72,8 @@ RobotDynamicsEstimator::RobotDynamicsEstimator() RobotDynamicsEstimator::~RobotDynamicsEstimator() = default; -bool RobotDynamicsEstimator::initialize(std::weak_ptr handler) +bool RobotDynamicsEstimator::initialize( + std::weak_ptr handler) { constexpr auto logPrefix = "[RobotDynamicsEstimator::initialize]"; @@ -144,10 +155,11 @@ bool RobotDynamicsEstimator::finalize(const System::VariablesHandler& stateVaria 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) +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]"; @@ -161,8 +173,10 @@ std::unique_ptr RobotDynamicsEstimator::build(std::weak_ 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); + log()->error("{} Cannot set the frame velocity representation on the " + "`iDynTree::KinDynComputation` " + "object describing the full robot model.", + logPrefix); return nullptr; } @@ -193,10 +207,8 @@ std::unique_ptr RobotDynamicsEstimator::build(std::weak_ // Step 1 // Create the state model for the ukf - estimator->m_pimpl->stateModel = UkfState::build(groupUKFState, - kinDynFullModel, - subModelList, - kinDynWrapperList); + 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); @@ -210,15 +222,42 @@ std::unique_ptr RobotDynamicsEstimator::build(std::weak_ estimator->m_pimpl->stateModel->setUkfInputProvider(estimator->m_pimpl->inputProvider); // Get initial state covariance - estimator->m_pimpl->initialStateCovariance = estimator->m_pimpl->stateModel->getInitialStateCovarianceMatrix(); + 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); + // 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); + + std::vector dynamicsList; + if (!groupUKFState->getParameter("dynamics_list", dynamicsList)) + { + log()->error("{} Unable to find the parameter 'dynamics_list'.", logPrefix); + return nullptr; + } + for (auto const& dynamicsName : dynamicsList) + { + auto dynamicsGroup = groupUKFState->getGroup(dynamicsName).lock(); + if (dynamicsGroup == nullptr) + { + log()->error("{} Unable to find the group '{}'.", logPrefix, dynamicsName); + return nullptr; + } + std::string inputName; + if (!dynamicsGroup->getParameter("input_name", inputName)) + { + log()->error("{} Unable to find the parameter 'covariance'.", logPrefix); + return nullptr; + } + estimator->m_pimpl->inputNameToUkfState[inputName] = dynamicsName; + } // Step 3 // Create the measurement model for the ukf @@ -238,6 +277,10 @@ std::unique_ptr RobotDynamicsEstimator::build(std::weak_ kinDynFullModel, subModelList, kinDynWrapperList); + + estimator->m_pimpl->measurementModel->setStateNameMapping( + estimator->m_pimpl->inputNameToUkfState); + if (estimator->m_pimpl->measurementModel == nullptr) { log()->error("{} Error while creating the ukf measurement model.", logPrefix); @@ -245,21 +288,49 @@ std::unique_ptr RobotDynamicsEstimator::build(std::weak_ } // Save a copy of the measurement variable handler - estimator->m_pimpl->measurementHandler = estimator->m_pimpl->measurementModel->getMeasurementVariableHandler(); + 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); + // 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); + + if (!groupUKFMeas->getParameter("dynamics_list", dynamicsList)) + { + log()->error("{} Unable to find the parameter 'dynamics_list'.", logPrefix); + return nullptr; + } + for (auto const& dynamicsName : dynamicsList) + { + auto dynamicsGroup = groupUKFMeas->getGroup(dynamicsName).lock(); + if (dynamicsGroup == nullptr) + { + log()->error("{} Unable to find the group '{}'.", logPrefix, dynamicsName); + return nullptr; + } + std::string inputName; + if (!dynamicsGroup->getParameter("input_name", inputName)) + { + log()->error("{} Unable to find the parameter 'covariance'.", logPrefix); + return nullptr; + } + estimator->m_pimpl->inputNameToUkfMeasurement[inputName].push_back(dynamicsName); + } // Finalize the estimator - estimator->finalize(estimator->m_pimpl->stateHandler, estimator->m_pimpl->measurementHandler, kinDynFullModel); + estimator->finalize(estimator->m_pimpl->stateHandler, + estimator->m_pimpl->measurementHandler, + kinDynFullModel); return estimator; } @@ -270,43 +341,62 @@ bool RobotDynamicsEstimator::setInitialState(const RobotDynamicsEstimatorOutput& System::VariablesHandler::VariableDescription variable; - if (m_pimpl->stateHandler.getVariable("ds", variable)) + if (!m_pimpl->stateHandler.getVariable("JOINT_VELOCITIES", 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; + log()->error("{} Variable `JOINT_VELOCITIES` not found.", logPrefix); + return false; + } + 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 (!m_pimpl->stateHandler.getVariable("MOTOR_TORQUES", 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; + log()->error("{} Variable `MOTOR_TORQUES` not found.", logPrefix); + return false; + } + 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 (!m_pimpl->stateHandler.getVariable("FRICTION_TORQUES", 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; + log()->error("{} Variable `FRICTION_TORQUES` not found.", logPrefix); + return false; } + 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 (m_pimpl->stateHandler.getVariable(m_pimpl->inputNameToUkfState[key], variable)) { if (val.size() != variable.size) { - log()->error("{} Wrong size of variable `{}`. Found {}, expected {}.", logPrefix, val, 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; @@ -315,11 +405,15 @@ bool RobotDynamicsEstimator::setInitialState(const RobotDynamicsEstimatorOutput& for (auto const& [key, val] : initialState.ftWrenchesBiases) { - if (m_pimpl->stateHandler.getVariable(key, variable)) + if (m_pimpl->stateHandler.getVariable(m_pimpl->inputNameToUkfState[key], variable)) { if (val.size() != variable.size) { - log()->error("{} Wrong size of variable `{}`. Found {}, expected {}.", logPrefix, val, 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; @@ -328,11 +422,15 @@ bool RobotDynamicsEstimator::setInitialState(const RobotDynamicsEstimatorOutput& for (auto const& [key, val] : initialState.gyroscopeBiases) { - if (m_pimpl->stateHandler.getVariable(key, variable)) + if (m_pimpl->stateHandler.getVariable(m_pimpl->inputNameToUkfState[key], variable)) { if (val.size() != variable.size) { - log()->error("{} Wrong size of variable `{}`. Found {}, expected {}.", logPrefix, val, 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; @@ -341,11 +439,15 @@ bool RobotDynamicsEstimator::setInitialState(const RobotDynamicsEstimatorOutput& for (auto const& [key, val] : initialState.accelerometerBiases) { - if (m_pimpl->stateHandler.getVariable(key, variable)) + if (m_pimpl->stateHandler.getVariable(m_pimpl->inputNameToUkfState[key], variable)) { if (val.size() != variable.size) { - log()->error("{} Wrong size of variable `{}`. Found {}, expected {}.", logPrefix, val, 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; @@ -354,11 +456,49 @@ bool RobotDynamicsEstimator::setInitialState(const RobotDynamicsEstimatorOutput& for (auto const& [key, val] : initialState.contactWrenches) { - if (m_pimpl->stateHandler.getVariable(key, variable)) + if (m_pimpl->stateHandler.getVariable(m_pimpl->inputNameToUkfState[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.linearAccelerations) + { + if (m_pimpl->stateHandler.getVariable(m_pimpl->inputNameToUkfState[key], variable)) { if (val.size() != variable.size) { - log()->error("{} Wrong size of variable `{}`. Found {}, expected {}.", logPrefix, val, 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.angularVelocities) + { + if (m_pimpl->stateHandler.getVariable(m_pimpl->inputNameToUkfState[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; @@ -420,7 +560,7 @@ bool RobotDynamicsEstimator::advance() return true; } -bool RobotDynamicsEstimator::setInput(const RobotDynamicsEstimatorInput & input) +bool RobotDynamicsEstimator::setInput(const RobotDynamicsEstimatorInput& input) { constexpr auto logPrefix = "[RobotDynamicsEstimator::setInput]"; @@ -439,103 +579,150 @@ bool RobotDynamicsEstimator::setInput(const RobotDynamicsEstimatorInput & input) // 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["JOINT_VELOCITIES"] = input.jointVelocities; + m_pimpl->ukfMeasurementFromSensors["MOTOR_CURRENTS"] = input.motorCurrents; + for (const auto& [key, value] : input.ftWrenches) { - m_pimpl->ukfMeasurementFromSensors[key] = value; + for (int index = 0; index < m_pimpl->inputNameToUkfMeasurement[key].size(); index++) + { + m_pimpl->ukfMeasurementFromSensors[m_pimpl->inputNameToUkfMeasurement[key][index]] = value; + } } - for (auto & [key, value] : input.linearAccelerations) + for (auto& [key, value] : input.linearAccelerations) { - m_pimpl->ukfMeasurementFromSensors[key] = value; + for (int index = 0; index < m_pimpl->inputNameToUkfMeasurement[key].size(); index++) + { + m_pimpl->ukfMeasurementFromSensors[m_pimpl->inputNameToUkfMeasurement[key][index]] = value; + } } - for (auto & [key, value] : input.angularVelocities) + for (auto& [key, value] : input.angularVelocities) { - m_pimpl->ukfMeasurementFromSensors[key] = value; + for (int index = 0; index < m_pimpl->inputNameToUkfMeasurement[key].size(); index++) + { + m_pimpl->ukfMeasurementFromSensors[m_pimpl->inputNameToUkfMeasurement[key][index]] = value; + } } - m_pimpl->ukfMeasurementFromSensors["tau_F"] = input.friction; + m_pimpl->ukfMeasurementFromSensors["FRICTION_TORQUES"] = input.frictionTorques; return true; } const RobotDynamicsEstimatorOutput& RobotDynamicsEstimator::getOutput() const { - constexpr auto logPrefix = "[RobotDynamicsEstimator::getOutput]"; + 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.ds + = m_pimpl->correctedState.mean() + .segment(m_pimpl->stateHandler.getVariable("JOINT_VELOCITIES").offset, + m_pimpl->stateHandler.getVariable("JOINT_VELOCITIES").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_m + = m_pimpl->correctedState.mean() + .segment(m_pimpl->stateHandler.getVariable("MOTOR_TORQUES").offset, + m_pimpl->stateHandler.getVariable("MOTOR_TORQUES").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); + m_pimpl->estimatorOutput.tau_F + = m_pimpl->correctedState.mean() + .segment(m_pimpl->stateHandler.getVariable("FRICTION_TORQUES").offset, + m_pimpl->stateHandler.getVariable("FRICTION_TORQUES").size); - for (auto & [key, value] : m_pimpl->estimatorOutput.ftWrenches) + for (auto& [key, value] : m_pimpl->estimatorOutput.ftWrenches) { - if (m_pimpl->stateHandler.getVariable(key).size > 0) + if (m_pimpl->stateHandler.getVariable(m_pimpl->inputNameToUkfState[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 + m_pimpl->estimatorOutput.ftWrenches[key] + = m_pimpl->correctedState.mean() + .segment(m_pimpl->stateHandler + .getVariable(m_pimpl->inputNameToUkfState[key]) + .offset, + m_pimpl->stateHandler + .getVariable(m_pimpl->inputNameToUkfState[key]) + .size); + } else { log()->debug("{} Variable {} not found in the state vector.", logPrefix, key); } } - for (auto & [key, value] : m_pimpl->estimatorOutput.ftWrenchesBiases) + for (auto& [key, value] : m_pimpl->estimatorOutput.ftWrenchesBiases) { - if (m_pimpl->stateHandler.getVariable(key).size > 0) + if (m_pimpl->stateHandler.getVariable(m_pimpl->inputNameToUkfState[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 + m_pimpl->estimatorOutput.ftWrenchesBiases[key] + = m_pimpl->correctedState.mean() + .segment(m_pimpl->stateHandler + .getVariable(m_pimpl->inputNameToUkfState[key]) + .offset, + m_pimpl->stateHandler + .getVariable(m_pimpl->inputNameToUkfState[key]) + .size); + } else { - log()->debug("{} Variable {} not found in the state vector.", logPrefix, key); + log()->debug("{} Variable {} not found in the state vector.", + logPrefix, + m_pimpl->inputNameToUkfState[key]); } } - for (auto & [key, value] : m_pimpl->estimatorOutput.accelerometerBiases) + for (auto& [key, value] : m_pimpl->estimatorOutput.accelerometerBiases) { - if (m_pimpl->stateHandler.getVariable(key).size > 0) + if (m_pimpl->stateHandler.getVariable(m_pimpl->inputNameToUkfState[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 + m_pimpl->estimatorOutput.accelerometerBiases[key] + = m_pimpl->correctedState.mean() + .segment(m_pimpl->stateHandler + .getVariable(m_pimpl->inputNameToUkfState[key]) + .offset, + m_pimpl->stateHandler + .getVariable(m_pimpl->inputNameToUkfState[key]) + .size); + } else { - log()->debug("{} Variable {} not found in the state vector.", logPrefix, key); + log()->debug("{} Variable {} not found in the state vector.", + logPrefix, + m_pimpl->inputNameToUkfState[key]); } } - for (auto & [key, value] : m_pimpl->estimatorOutput.gyroscopeBiases) + for (auto& [key, value] : m_pimpl->estimatorOutput.gyroscopeBiases) { - if (m_pimpl->stateHandler.getVariable(key).size > 0) + if (m_pimpl->stateHandler.getVariable(m_pimpl->inputNameToUkfState[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 + m_pimpl->estimatorOutput.gyroscopeBiases[key] + = m_pimpl->correctedState.mean() + .segment(m_pimpl->stateHandler + .getVariable(m_pimpl->inputNameToUkfState[key]) + .offset, + m_pimpl->stateHandler + .getVariable(m_pimpl->inputNameToUkfState[key]) + .size); + } else { - log()->debug("{} Variable {} not found in the state vector.", logPrefix, key); + log()->debug("{} Variable {} not found in the state vector.", + logPrefix, + m_pimpl->inputNameToUkfState[key]); } } - for (auto & [key, value] : m_pimpl->estimatorOutput.contactWrenches) + for (auto& [key, value] : m_pimpl->estimatorOutput.contactWrenches) { - if (m_pimpl->stateHandler.getVariable(key).size > 0) + if (m_pimpl->stateHandler.getVariable(m_pimpl->inputNameToUkfState[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 + m_pimpl->estimatorOutput.contactWrenches[key] + = m_pimpl->correctedState.mean() + .segment(m_pimpl->stateHandler + .getVariable(m_pimpl->inputNameToUkfState[key]) + .offset, + m_pimpl->stateHandler + .getVariable(m_pimpl->inputNameToUkfState[key]) + .size); + } else { - log()->debug("{} Variable {} not found in the state vector.", logPrefix, key); + log()->debug("{} Variable {} not found in the state vector.", + logPrefix, + m_pimpl->inputNameToUkfState[key]); } } } diff --git a/src/Estimators/src/SubModel.cpp b/src/Estimators/src/SubModel.cpp index 310419d8b2..5b32af7f4f 100644 --- a/src/Estimators/src/SubModel.cpp +++ b/src/Estimators/src/SubModel.cpp @@ -99,7 +99,6 @@ RDE::SubModelCreator::attachFTsToSubModel(const std::vector& ftLi RDE::FTSensor ft; ft.name = ftFromConfig.name; ft.frame = ftFromConfig.frame; - ft.ukfName = ftFromConfig.ukfName; // Retrieve force direction if (idynSubModel.isLinkNameUsed(linkAppliedWrenchName)) @@ -144,7 +143,6 @@ RDE::SubModelCreator::attachFTsToSubModel(const std::vector& ftLi RDE::FTSensor ft; ft.name = ftFromConfig.name; ft.frame = ftFromConfig.frame; - ft.ukfName = ftFromConfig.ukfName; ft.frameIndex = idynSubModel.getFrameIndex(ft.frame); @@ -176,7 +174,6 @@ std::unordered_map RDE::SubModelCreator::attachAcceler RDE::Sensor acc; acc.name = accListFromConfig[idx].name; acc.frame = accListFromConfig[idx].frame; - acc.ukfName = accListFromConfig[idx].ukfName; acc.frameIndex = subModel.getFrameIndex(acc.frame); accList[acc.name] = std::move(acc); } @@ -198,7 +195,6 @@ RDE::SubModelCreator::attachGyroscopesToSubModel(const std::vector& RDE::Sensor gyro; gyro.name = gyroListFromConfig[idx].name; gyro.frame = gyroListFromConfig[idx].frame; - gyro.ukfName = gyroListFromConfig[idx].ukfName; gyro.frameIndex = subModel.getFrameIndex(gyro.frame); gyroList[gyro.name] = std::move(gyro); } @@ -219,7 +215,6 @@ std::unordered_map RDE::SubModelCreator::attachExterna RDE::Sensor contact; contact.name = contactsFromConfig[idx].name; contact.frame = contactsFromConfig[idx].frame; - contact.ukfName = contactsFromConfig[idx].ukfName; contact.frameIndex = subModel.getFrameIndex(contact.frame); contactList[contact.name] = std::move(contact); } @@ -266,6 +261,28 @@ RDE::SubModelCreator::populateSubModel(iDynTree::Model& idynSubModel, subModel.m_jointListMapping = RDE::SubModelCreator::createJointMapping(idynSubModel); + std::string baseLink = idynSubModel.getLinkName(0); + + // The first IMU found in the model is used as base + int frameIdx = 0; + bool frameFound = false; + while(!frameFound && frameIdx < idynSubModel.getNrOfFrames()) + { + std::string frameName = idynSubModel.getFrameName(frameIdx); + + for (const auto& acc : subModel.m_accelerometerList) + { + if (acc.second.frame == frameName) + { + subModel.m_imuBaseFrameIndex = frameIdx; + subModel.m_imuBaseFrameName = frameName; + frameFound = true; + break; + } + } + frameIdx++; + } + return subModel; } @@ -329,8 +346,7 @@ bool RDE::SubModelCreator::createSubModels( auto populateSensorParameters = [&ptr, logPrefix](const std::string& groupName, std::vector& names, - std::vector& frames, - std::vector& ukfNames) -> bool { + std::vector& frames) -> bool { auto group = ptr->getGroup(groupName).lock(); if (group == nullptr) { @@ -356,20 +372,11 @@ bool RDE::SubModelCreator::createSubModels( return false; } - if (!group->getParameter("ukf_names", ukfNames)) - { - log()->error("{} The parameter handler could not find 'ukf_names' in the " - "configuration file for the group {}.", - logPrefix, - groupName); - return false; - } - return true; }; - std::vector ftNames, ftFrames, ftAssociatedJoints, ftUkfNames; - bool ok = populateSensorParameters("FT", ftNames, ftFrames, ftUkfNames); + std::vector ftNames, ftFrames, ftAssociatedJoints; + bool ok = populateSensorParameters("FT", ftNames, ftFrames); auto ftGroup = ptr->getGroup("FT").lock(); if (ftGroup == nullptr) @@ -385,13 +392,12 @@ bool RDE::SubModelCreator::createSubModels( RDE::FTSensor ft; ft.name = ftNames[idx]; ft.frame = ftFrames[idx]; - ft.ukfName = ftUkfNames[idx]; ft.associatedJoint = ftAssociatedJoints[idx]; ftList.push_back(std::move(ft)); } - std::vector accNames, accFrames, accUkfNames; - ok = ok && populateSensorParameters("ACCELEROMETER", accNames, accFrames, accUkfNames); + std::vector accNames, accFrames; + ok = ok && populateSensorParameters("ACCELEROMETER", accNames, accFrames); std::vector accList; for (auto idx = 0; idx < accNames.size(); idx++) @@ -399,12 +405,11 @@ bool RDE::SubModelCreator::createSubModels( RDE::Sensor acc; acc.name = accNames[idx]; acc.frame = accFrames[idx]; - acc.ukfName = accUkfNames[idx]; accList.push_back(std::move(acc)); } - std::vector gyroNames, gyroFrames, gyroUkfNames; - ok = ok && populateSensorParameters("GYROSCOPE", gyroNames, gyroFrames, gyroUkfNames); + std::vector gyroNames, gyroFrames; + ok = ok && populateSensorParameters("GYROSCOPE", gyroNames, gyroFrames); std::vector gyroList; for (auto idx = 0; idx < gyroNames.size(); idx++) @@ -412,16 +417,14 @@ bool RDE::SubModelCreator::createSubModels( RDE::Sensor gyro; gyro.name = gyroNames[idx]; gyro.frame = gyroFrames[idx]; - gyro.ukfName = gyroUkfNames[idx]; gyroList.push_back(std::move(gyro)); } - std::vector contactNames, contactFrames, contactUkfNames; + std::vector contactNames, contactFrames; ok = ok && populateSensorParameters("EXTERNAL_CONTACT", contactNames, - contactFrames, - contactUkfNames); + contactFrames); std::vector contactList; for (auto idx = 0; idx < contactNames.size(); idx++) @@ -429,7 +432,6 @@ bool RDE::SubModelCreator::createSubModels( RDE::Sensor contact; contact.name = contactNames[idx]; contact.frame = contactFrames[idx]; - contact.ukfName = contactUkfNames[idx]; contactList.push_back(std::move(contact)); } @@ -594,3 +596,13 @@ const RDE::Sensor& RDE::SubModel::getExternalContactIndex(const std::string& nam return dummySensor; } + +const std::string RDE::SubModel::getImuBaseFrameName() const +{ + return m_imuBaseFrameName; +} + +int RDE::SubModel::getImuBaseFrameIndex() const +{ + return m_imuBaseFrameIndex; +} diff --git a/src/Estimators/src/UkfMeasurement.cpp b/src/Estimators/src/UkfMeasurement.cpp index ab70c219fe..212dddd63f 100644 --- a/src/Estimators/src/UkfMeasurement.cpp +++ b/src/Estimators/src/UkfMeasurement.cpp @@ -136,6 +136,11 @@ bool UkfMeasurement::finalize(const System::VariablesHandler& handler) return true; } +void UkfMeasurement::setStateNameMapping(const std::map& stateToUkfNames) +{ + m_stateToUkfNames = stateToUkfNames; +} + std::unique_ptr UkfMeasurement::build(std::weak_ptr handler, System::VariablesHandler& stateVariableHandler, @@ -209,13 +214,14 @@ UkfMeasurement::build(std::weak_ptr dynamicsGroup->setParameter("sampling_time", measurement->m_dT); // create variable handler - std::string dynamicsName; - std::vector covariances; - if (!dynamicsGroup->getParameter("name", dynamicsName)) + std::string inputName; + if (!dynamicsGroup->getParameter("input_name", inputName)) { - BipedalLocomotion::log()->error("{} Unable to find the parameter 'name'.", logPrefix); + BipedalLocomotion::log()->error("{} Unable to find the parameter 'input_name'.", + logPrefix); return nullptr; } + std::vector covariances; if (!dynamicsGroup->getParameter("covariance", covariances)) { BipedalLocomotion::log()->error("{} Unable to find the parameter 'covariance'.", @@ -238,16 +244,18 @@ UkfMeasurement::build(std::weak_ptr "the measurement variable `{}`.", logPrefix, dynamicModel, - dynamicsName); + dynamicsGroupName); return nullptr; } dynamicsInstance->setSubModels(subModelList, kinDynWrapperList); - dynamicsInstance->initialize(dynamicsGroup); + dynamicsInstance->initialize(dynamicsGroup, dynamicsGroupName); // add dynamics to the list - measurement->m_dynamicsList.emplace_back(dynamicsName, dynamicsInstance); + measurement->m_dynamicsList.emplace_back(dynamicsGroupName, dynamicsInstance); + + measurement->m_ukfNamesToMeasures[dynamicsGroupName] = inputName; } measurement->m_inputDescription @@ -324,7 +332,7 @@ UkfMeasurement::predictedMeasure(const Eigen::Ref& curren const_cast(this)->m_ukfInput.robotJointAccelerations = m_jointAccelerationState; - for (int indexDyn = 0; indexDyn < m_dynamicsList.size(); indexDyn++) + for (int indexDyn = 0; indexDyn < m_dynamicsList.size(); indexDyn++) { m_dynamicsList[indexDyn].second->setState(m_currentState); @@ -394,31 +402,28 @@ bool UkfMeasurement::freeze(const bfl::Data& data) m_measurementMap = bfl::any::any_cast>(data); - for (int index = 0; index < m_dynamicsList.size(); index++) + for (const auto& [key, value] : m_ukfNamesToMeasures) { - m_offsetMeasurement - = m_measurementVariableHandler.getVariable(m_dynamicsList[index].first).offset; + m_offsetMeasurement = m_measurementVariableHandler.getVariable(key).offset; - if (m_measurementMap.count(m_dynamicsList[index].first) == 0) + if (m_measurementMap.count(key) == 0) { BipedalLocomotion::log()->error("{} Measurement with name `{}` not found.", logPrefix, - m_dynamicsList[index].first); + key); 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)) + while (m_offsetMeasurement < (m_measurementVariableHandler.getVariable(key).offset + + m_measurementVariableHandler.getVariable(key).size)) { - m_measurement.segment(m_offsetMeasurement, - m_measurementMap[m_dynamicsList[index].first].size()) - = m_measurementMap[m_dynamicsList[index].first]; + m_measurement.segment(m_offsetMeasurement, m_measurementMap[key].size()) + = m_measurementMap[key]; - m_offsetMeasurement += m_measurementMap[m_dynamicsList[index].first].size(); + m_offsetMeasurement += m_measurementMap[key].size(); } } diff --git a/src/Estimators/src/UkfModel.cpp b/src/Estimators/src/UkfModel.cpp index 74157152e3..49730ff34e 100644 --- a/src/Estimators/src/UkfModel.cpp +++ b/src/Estimators/src/UkfModel.cpp @@ -18,8 +18,9 @@ using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; void UkfModel::unpackState() { - m_jointVelocityState = m_currentState.segment(m_stateVariableHandler.getVariable("ds").offset, - m_stateVariableHandler.getVariable("ds").size); + m_jointVelocityState + = m_currentState.segment(m_stateVariableHandler.getVariable("JOINT_VELOCITIES").offset, + m_stateVariableHandler.getVariable("JOINT_VELOCITIES").size); for (int subModelIdx = 0; subModelIdx < m_subModelList.size(); subModelIdx++) { @@ -32,26 +33,44 @@ void UkfModel::unpackState() = m_jointVelocityState(m_subModelList[subModelIdx].getJointMapping()[jointIdx]); m_subModelJointMotorTorque[subModelIdx](jointIdx) - = m_currentState[m_stateVariableHandler.getVariable("tau_m").offset + = m_currentState[m_stateVariableHandler.getVariable("MOTOR_TORQUES").offset + m_subModelList[subModelIdx].getJointMapping()[jointIdx]]; m_subModelFrictionTorque[subModelIdx](jointIdx) - = m_currentState[m_stateVariableHandler.getVariable("tau_F").offset + = m_currentState[m_stateVariableHandler.getVariable("FRICTION_TORQUES").offset + m_subModelList[subModelIdx].getJointMapping()[jointIdx]]; } for (auto& [key, value] : m_subModelList[subModelIdx].getFTList()) { m_FTMap[key] - = m_currentState.segment(m_stateVariableHandler.getVariable(value.ukfName).offset, - m_stateVariableHandler.getVariable(value.ukfName).size); + = m_currentState + .segment(m_stateVariableHandler.getVariable(m_stateToUkfNames[key]).offset, + m_stateVariableHandler.getVariable(m_stateToUkfNames[key]).size); } for (auto& [key, value] : m_subModelList[subModelIdx].getExternalContactList()) { m_extContactMap[key] - = m_currentState.segment(m_stateVariableHandler.getVariable(value.ukfName).offset, - m_stateVariableHandler.getVariable(value.ukfName).size); + = m_currentState + .segment(m_stateVariableHandler.getVariable(m_stateToUkfNames[key]).offset, + m_stateVariableHandler.getVariable(m_stateToUkfNames[key]).size); + } + + for (const auto& [key, value] : m_subModelList[subModelIdx].getAccelerometerList()) + { + m_accMap[key] + = m_currentState + .segment(m_stateVariableHandler.getVariable(m_stateToUkfNames[key]).offset, + m_stateVariableHandler.getVariable(m_stateToUkfNames[key]).size); + } + + for (const auto& [key, value] : m_subModelList[subModelIdx].getGyroscopeList()) + { + m_gyroMap[key] + = m_currentState + .segment(m_stateVariableHandler.getVariable(m_stateToUkfNames[key]).offset, + m_stateVariableHandler.getVariable(m_stateToUkfNames[key]).size); } } } @@ -60,22 +79,45 @@ bool UkfModel::updateState() { constexpr auto logPrefix = "[UkfModel::updateState]"; - // Update kindyn full model + m_baseVelocity.setZero(); + m_baseAcceleration.setZero(); + + // The full model shares the same base as the first submodel + for (const auto& [key, value] : m_subModelList[0].getGyroscopeList()) + { + if (m_subModelList[0].getImuBaseFrameName() == value.frame) + { + m_baseVelocity.coeffs().tail(3) = m_gyroMap[key]; + } + } + + manif::SE3d baseHimu; // Transform from the base frame to the imu frame. + + // Get transform matrix from imu to base + baseHimu = Conversions::toManifPose( + m_kinDynFullModel->getRelativeTransform(m_kinDynFullModel->getFloatingBase(), + m_subModelList[0].getImuBaseFrameName())); + + m_baseVelocity.coeffs().tail(3).noalias() = baseHimu.rotation() * m_baseVelocity.coeffs().tail(3); + + m_gravity.setZero(); + + // Set the robot state of the kindyn of the full model using the sensor proper acceleration m_kinDynFullModel->setRobotState(m_ukfInput.robotBasePose.transform(), m_ukfInput.robotJointPositions, - iDynTree::make_span(m_ukfInput.robotBaseVelocity.data(), + iDynTree::make_span(m_baseVelocity.data(), manif::SE3d::Tangent::DoF), m_jointVelocityState, m_gravity); - // compute joint acceleration per each sub-model + // Compute acceleration of the submodel bases from imu measurements for (int subModelIdx = 0; subModelIdx < m_subModelList.size(); subModelIdx++) { - // Get sub-model base pose + // Get sub-model base pose expressed in the world frame that is used to set the kindyn state of the submodel m_worldTBase = Conversions::toManifPose(m_kinDynFullModel->getWorldTransform( m_kinDynWrapperList[subModelIdx]->getFloatingBase())); - // Get sub-model joint position + // Get sub-model joint positions used to set the kindyn state of the submodel for (int jointIdx = 0; jointIdx < m_subModelList[subModelIdx].getModel().getNrOfDOFs(); jointIdx++) { @@ -84,23 +126,46 @@ bool UkfModel::updateState() .robotJointPositions[m_subModelList[subModelIdx].getJointMapping()[jointIdx]]; } - // Get sub-model base vel - if (!m_kinDynFullModel->getFrameVel(m_kinDynWrapperList[subModelIdx]->getFloatingBase(), - iDynTree::make_span(m_subModelBaseVelTemp.data(), - manif::SE3d::Tangent::DoF))) + // Get the transform matrix from imu to the base frame of the submodel + baseHimu = Conversions::toManifPose( + m_kinDynWrapperList[subModelIdx] + ->getRelativeTransform(m_kinDynWrapperList[subModelIdx]->getFloatingBase(), + m_subModelList[subModelIdx].getImuBaseFrameName())); + + // Set the base velocity of the submodel from the gyroscope measurement rotating it in the base frame + for (const auto& [key, value] : m_subModelList[subModelIdx].getGyroscopeList()) { - log()->error("{} Failed while getting the base frame velocity.", logPrefix); - return false; + if (m_subModelList[subModelIdx].getImuBaseFrameName() == value.frame) + { + m_baseVelocity.coeffs().tail(3) = baseHimu.rotation() * m_gyroMap[key]; + } + } + + m_bOmegaIB = m_baseVelocity.coeffs().tail(3); + + for (const auto& [key, value] : m_subModelList[subModelIdx].getAccelerometerList()) + { + if (m_subModelList[subModelIdx].getImuBaseFrameName() == value.frame) + { + m_tempAccelerometerVelocity = Conversions::toManifTwist( + m_kinDynWrapperList[subModelIdx]->getFrameVel( + value.frame)); + + // See reference: https://traversaro.github.io/traversaro-phd-thesis/traversaro-phd-thesis.pdf + // Paragraph 4.4.2 + m_baseAcceleration.coeffs().head(3).noalias() + = baseHimu.rotation() * m_accMap[key] + - m_bOmegaIB.cross(m_bOmegaIB.cross(baseHimu.translation())); + } } - // 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); + // Set robot state + m_kinDynWrapperList[subModelIdx]->setRobotState(m_worldTBase.transform(), + m_subModelJointPos[subModelIdx], + iDynTree::make_span(m_baseVelocity.data(), + manif::SE3d::Tangent::DoF), + m_subModelJointVel[subModelIdx], + m_gravity); m_totalTorqueFromContacts[subModelIdx].setZero(); @@ -141,39 +206,16 @@ bool UkfModel::updateState() += 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_baseAcceleration, + m_subModelJointAcc[subModelIdx])) { - 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()); + log()->error("{} Cannot compute the inverse dynamics.", logPrefix); + return false; } // Assign joint acceleration using the correct indeces diff --git a/src/Estimators/src/UkfState.cpp b/src/Estimators/src/UkfState.cpp index f5b276cac5..0812009174 100644 --- a/src/Estimators/src/UkfState.cpp +++ b/src/Estimators/src/UkfState.cpp @@ -191,13 +191,13 @@ RDE::UkfState::build(std::weak_ptr dynamicsGroup->setParameter("sampling_time", state->m_dT); // create variable handler - std::string dynamicsName; - std::vector covariances; - if (!dynamicsGroup->getParameter("name", dynamicsName)) + std::string inputName; + if (!dynamicsGroup->getParameter("input_name", inputName)) { - log()->error("{} Unable to find the parameter 'name'.", logPrefix); + log()->error("{} Unable to find the parameter 'input_name'.", logPrefix); return nullptr; } + std::vector covariances; if (!dynamicsGroup->getParameter("covariance", covariances)) { log()->error("{} Unable to find the parameter 'covariance'.", logPrefix); @@ -208,7 +208,7 @@ RDE::UkfState::build(std::weak_ptr log()->error("{} Unable to find the parameter 'initial_covariance'.", logPrefix); return nullptr; } - state->m_stateVariableHandler.addVariable(dynamicsName, covariances.size()); + state->m_stateVariableHandler.addVariable(dynamicsGroupName, covariances.size()); std::string dynamicModel; if (!dynamicsGroup->getParameter("dynamic_model", dynamicModel)) @@ -225,16 +225,18 @@ RDE::UkfState::build(std::weak_ptr "`{}`.", logPrefix, dynamicModel, - dynamicsName); + dynamicsGroupName); return nullptr; } dynamicsInstance->setSubModels(subModelList, kinDynWrapperList); - dynamicsInstance->initialize(dynamicsGroup); + dynamicsInstance->initialize(dynamicsGroup, dynamicsGroupName); // add dynamics to the list - state->m_dynamicsList.emplace_back(dynamicsName, dynamicsInstance); + state->m_dynamicsList.emplace_back(dynamicsGroupName, dynamicsInstance); + + state->m_stateToUkfNames[inputName] = dynamicsGroupName; } // finalize estimator @@ -338,7 +340,7 @@ Eigen::Ref RDE::UkfState::getInitialStateCovarianceMatrix return m_initialCovariance; } - bool RDE::UkfState::setProperty(const std::string& property) - { +bool RDE::UkfState::setProperty(const std::string& property) +{ return false; - } +} diff --git a/src/Estimators/src/ZeroVelocityStateDynamics.cpp b/src/Estimators/src/ZeroVelocityStateDynamics.cpp index 19edde479e..db48997fd5 100644 --- a/src/Estimators/src/ZeroVelocityStateDynamics.cpp +++ b/src/Estimators/src/ZeroVelocityStateDynamics.cpp @@ -5,12 +5,14 @@ * distributed under the terms of the BSD-3-Clause license. */ -#include #include +#include namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; -bool RDE::ZeroVelocityStateDynamics::initialize(std::weak_ptr paramHandler) +bool RDE::ZeroVelocityStateDynamics::initialize( + std::weak_ptr paramHandler, + const std::string& name) { constexpr auto errorPrefix = "[ZeroVelocityStateDynamics::initialize]"; @@ -21,12 +23,7 @@ bool RDE::ZeroVelocityStateDynamics::initialize(std::weak_ptrgetParameter("name", m_name)) - { - log()->error("{} Error while retrieving the name variable.", errorPrefix); - return false; - } + m_name = name; // Set the state process covariance if (!ptr->getParameter("covariance", m_covariances)) @@ -55,7 +52,7 @@ bool RDE::ZeroVelocityStateDynamics::initialize(std::weak_ptr& /*subModelList*/, const std::vector>& /*kinDynWrapperList*/) +bool RDE::ZeroVelocityStateDynamics::setSubModels( + const std::vector& /*subModelList*/, + const std::vector>& /*kinDynWrapperList*/) { return true; } @@ -102,7 +101,9 @@ bool RDE::ZeroVelocityStateDynamics::checkStateVariableHandler() // Check if the variable handler contains the variables used by this dynamics if (!m_stateVariableHandler.getVariable(m_name).isValid()) { - log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_name); + log()->error("{} The variable handler does not contain the expected state with name `{}`.", + errorPrefix, + m_name); return false; } diff --git a/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp index 9823e034ac..2572a5ad25 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp @@ -126,10 +126,10 @@ IParametersHandler::shared_ptr createModelParameterHandler() void createUkfInput(VariablesHandler& stateVariableHandler, UKFInput& input) { - Eigen::VectorXd jointPos = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size); + Eigen::VectorXd jointPos = Eigen::VectorXd::Random(stateVariableHandler.getVariable("JOINT_VELOCITIES").size); input.robotJointPositions = jointPos; - Eigen::VectorXd jointAcc = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size); + Eigen::VectorXd jointAcc = Eigen::VectorXd::Random(stateVariableHandler.getVariable("JOINT_VELOCITIES").size); input.robotJointAccelerations = jointAcc; manif::SE3d basePose @@ -152,18 +152,18 @@ void createStateVector(UKFInput& input, { state.setZero(); - Eigen::VectorXd jointVel = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size); + Eigen::VectorXd jointVel = Eigen::VectorXd::Random(stateVariableHandler.getVariable("JOINT_VELOCITIES").size); - int offset = stateVariableHandler.getVariable("ds").offset; - int size = stateVariableHandler.getVariable("ds").size; + int offset = stateVariableHandler.getVariable("JOINT_VELOCITIES").offset; + int size = stateVariableHandler.getVariable("JOINT_VELOCITIES").size; for (int jointIndex = 0; jointIndex < size; jointIndex++) { state[offset + jointIndex] = jointVel(jointIndex); } // Compute joint torques from inverse dynamics on the full model - offset = stateVariableHandler.getVariable("tau_m").offset; - size = stateVariableHandler.getVariable("tau_m").size; + offset = stateVariableHandler.getVariable("MOTOR_TORQUES").offset; + size = stateVariableHandler.getVariable("MOTOR_TORQUES").size; iDynTree::LinkNetExternalWrenches extWrench(kinDyn->model()); extWrench.zero(); iDynTree::FreeFloatingGeneralizedTorques jointTorques(kinDyn->model()); @@ -193,8 +193,8 @@ void setRandomKinDynState(std::vector& subModelList, gravity.setZero(); gravity(2) = -BipedalLocomotion::Math::StandardAccelerationOfGravitation; - int offset = stateVariableHandler.getVariable("ds").offset; - int size = stateVariableHandler.getVariable("ds").size; + int offset = stateVariableHandler.getVariable("JOINT_VELOCITIES").offset; + int size = stateVariableHandler.getVariable("JOINT_VELOCITIES").size; Eigen::VectorXd jointVel = Eigen::VectorXd(size); for (int jointIndex = 0; jointIndex < size; jointIndex++) @@ -226,8 +226,8 @@ void setRandomKinDynState(std::vector& subModelList, // Get sub-model joint velocities subModelJointVel[0].resize(subModelList[0].getModel().getNrOfDOFs()); - offset = stateVariableHandler.getVariable("ds").offset; - size = stateVariableHandler.getVariable("ds").size; + offset = stateVariableHandler.getVariable("JOINT_VELOCITIES").offset; + size = stateVariableHandler.getVariable("JOINT_VELOCITIES").size; for (int jointIdx = 0; jointIdx < subModelList[0].getModel().getNrOfDOFs(); jointIdx++) { subModelJointVel[0](jointIdx) = state[offset + subModelList[0].getJointMapping()[jointIdx]]; @@ -242,8 +242,8 @@ void setRandomKinDynState(std::vector& subModelList, gravity); // Forward dynamics - offset = stateVariableHandler.getVariable("tau_m").offset; - size = stateVariableHandler.getVariable("tau_m").size; + offset = stateVariableHandler.getVariable("MOTOR_TORQUES").offset; + size = stateVariableHandler.getVariable("MOTOR_TORQUES").size; Eigen::VectorXd jointTrq(size); for (int jointIndex = 0; jointIndex < size; jointIndex++) @@ -276,9 +276,9 @@ TEST_CASE("Accelerometer Measurement Dynamics") // Create state variable handler constexpr size_t sizeVariable = 6; VariablesHandler stateVariableHandler; - REQUIRE(stateVariableHandler.addVariable("ds", sizeVariable)); - REQUIRE(stateVariableHandler.addVariable("tau_m", sizeVariable)); - REQUIRE(stateVariableHandler.addVariable("tau_F", sizeVariable)); + REQUIRE(stateVariableHandler.addVariable("JOINT_VELOCITIES", sizeVariable)); + REQUIRE(stateVariableHandler.addVariable("MOTOR_TORQUES", sizeVariable)); + REQUIRE(stateVariableHandler.addVariable("FRICTION_TORQUES", sizeVariable)); REQUIRE(stateVariableHandler.addVariable("r_leg_ft_acc_bias", 3)); // Create parameter handler to load the model @@ -309,7 +309,7 @@ TEST_CASE("Accelerometer Measurement Dynamics") // Create the dynamics AccelerometerMeasurementDynamics accDynamics; REQUIRE(accDynamics.setSubModels(subModelList, kinDynWrapperList)); - REQUIRE(accDynamics.initialize(accHandler)); + REQUIRE(accDynamics.initialize(accHandler, "r_leg_ft_acc")); REQUIRE(accDynamics.finalize(stateVariableHandler)); // Create an input for the ukf state diff --git a/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt b/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt index 3262e49b13..0a30246841 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt +++ b/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt @@ -24,7 +24,7 @@ add_bipedal_test(NAME ConstantMeasurementModel Eigen3::Eigen) if(BUILD_TESTING) - set(CUSTOM_MODEL_EXPECTED_MD5 33b903fe87939e2e4932564e45741047) + set(CUSTOM_MODEL_EXPECTED_MD5 c18dc5dafea6c968d0a3f2ecf20e0387) if (EXISTS "${CMAKE_CURRENT_BINARY_DIR}/model.urdf") file(MD5 "${CMAKE_CURRENT_BINARY_DIR}/model.urdf" CUSTOM_MODEL_CHECKSUM_VARIABLE) string(COMPARE EQUAL ${CUSTOM_MODEL_CHECKSUM_VARIABLE} ${CUSTOM_MODEL_EXPECTED_MD5} CUSTOM_MODEL_UPDATED) @@ -34,7 +34,7 @@ if(BUILD_TESTING) if(NOT ${CUSTOM_MODEL_UPDATED}) message(STATUS "Fetching CUSTOM model from github.com/ami-iit/paper_sorrentino_2024_icra_robot-dynamics-estimation/...") - file(DOWNLOAD https://raw.githubusercontent.com/ami-iit/paper_sorrentino_2024_icra_robot-dynamics-estimation/577c9d734ff0cb7128274b3880106da11678538c/urdf/four_joints_two_sensors.urdf + file(DOWNLOAD https://raw.githubusercontent.com/ami-iit/paper_sorrentino_2024_icra_robot-dynamics-estimation/main/python/urdf/four_joints_two_ft_three_imu.urdf ${CMAKE_CURRENT_BINARY_DIR}/model.urdf EXPECTED_MD5 ${CUSTOM_MODEL_EXPECTED_MD5}) endif() @@ -101,7 +101,7 @@ endif() if(FRAMEWORK_COMPILE_YarpImplementation AND FRAMEWORK_COMPILE_ManifConversions AND FRAMEWORK_COMPILE_matioCppConversions) if(BUILD_TESTING) set(DATASET_UPDATED FALSE) - set(DATASET_EXPECTED_MD5 ac0c11c75616f71ccd4d8858f9d063d8) + set(DATASET_EXPECTED_MD5 af2d4c57650ab5f9bf3613d12dc54614) if (EXISTS "${CMAKE_CURRENT_BINARY_DIR}/dataset.mat") file(MD5 "${CMAKE_CURRENT_BINARY_DIR}/dataset.mat" MAT_DATASET_CHECKSUM_VARIABLE) string(COMPARE EQUAL ${MAT_DATASET_CHECKSUM_VARIABLE} ${DATASET_EXPECTED_MD5} DATASET_UPDATED) @@ -111,7 +111,7 @@ if(FRAMEWORK_COMPILE_YarpImplementation AND FRAMEWORK_COMPILE_ManifConversions A if(NOT ${DATASET_UPDATED}) message(STATUS "Fetching synthetic dataset from https://github.com/ami-iit/paper_...") - file(DOWNLOAD https://github.com/ami-iit/paper_sorrentino_2024_icra_robot-dynamics-estimation/raw/main/python/dataset/synthetic_data/four_joints_two_ft_two_imu.mat?download= + file(DOWNLOAD https://github.com/ami-iit/paper_sorrentino_2024_icra_robot-dynamics-estimation/raw/main/python/dataset/synthetic_data/four_joints_two_ft_three_imu.mat?download= ${CMAKE_CURRENT_BINARY_DIR}/dataset.mat EXPECTED_MD5 ${DATASET_EXPECTED_MD5}) endif() diff --git a/src/Estimators/tests/RobotDynamicsEstimator/ConstantMeasurementModelTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/ConstantMeasurementModelTest.cpp index 6cabfdca91..68d2a53396 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/ConstantMeasurementModelTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/ConstantMeasurementModelTest.cpp @@ -25,14 +25,15 @@ TEST_CASE("Constant Measurement Model") // Create variable handler constexpr size_t sizeVariable = 6; VariablesHandler variableHandler; - REQUIRE(variableHandler.addVariable("ds", sizeVariable)); - REQUIRE(variableHandler.addVariable("tau_m", sizeVariable)); - REQUIRE(variableHandler.addVariable("tau_F", sizeVariable)); + REQUIRE(variableHandler.addVariable("JOINT_VELOCITIES", sizeVariable)); + REQUIRE(variableHandler.addVariable("MOTOR_TORQUES", sizeVariable)); + REQUIRE(variableHandler.addVariable("FRICTION_TORQUES", sizeVariable)); REQUIRE(variableHandler.addVariable("r_leg_ft", sizeVariable)); REQUIRE(variableHandler.addVariable("r_leg_ft_bias", sizeVariable)); REQUIRE(variableHandler.addVariable("r_foot_front_ft", sizeVariable)); - Eigen::VectorXd state = Eigen::VectorXd::Zero(sizeVariable * variableHandler.getNumberOfVariables()); + Eigen::VectorXd state + = Eigen::VectorXd::Zero(sizeVariable * variableHandler.getNumberOfVariables()); const std::string name = "r_leg_ft"; Eigen::VectorXd covariance(6); @@ -42,6 +43,7 @@ TEST_CASE("Constant Measurement Model") parameterHandler->clear(); parameterHandler->setParameter("name", name); + parameterHandler->setParameter("associated_state", name); parameterHandler->setParameter("covariance", covariance); parameterHandler->setParameter("initial_covariance", covariance); parameterHandler->setParameter("dynamic_model", model); @@ -50,7 +52,7 @@ TEST_CASE("Constant Measurement Model") ConstantMeasurementModel ft; - REQUIRE(ft.initialize(parameterHandler)); + REQUIRE(ft.initialize(parameterHandler, name)); REQUIRE(ft.finalize(variableHandler)); Eigen::VectorXd bias(6); @@ -59,13 +61,16 @@ TEST_CASE("Constant Measurement Model") bias(index) = GENERATE(take(1, random(-100, 100))); } - state.segment(variableHandler.getVariable("r_leg_ft_bias").offset, variableHandler.getVariable("r_leg_ft_bias").size) = bias; + state.segment(variableHandler.getVariable("r_leg_ft_bias").offset, + variableHandler.getVariable("r_leg_ft_bias").size) + = bias; ft.setState(state); REQUIRE(ft.update()); Eigen::VectorXd updatedVariable = ft.getUpdatedVariable(); - Eigen::VectorXd ftPre = state.segment(variableHandler.getVariable("r_leg_ft").offset, variableHandler.getVariable("r_leg_ft").size); + Eigen::VectorXd ftPre = state.segment(variableHandler.getVariable("r_leg_ft").offset, + variableHandler.getVariable("r_leg_ft").size); - REQUIRE((ftPre+bias).isApprox(updatedVariable)); + REQUIRE((ftPre + bias).isApprox(updatedVariable)); } diff --git a/src/Estimators/tests/RobotDynamicsEstimator/ExternalContactStateDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/ExternalContactStateDynamicsTest.cpp index 249f66f9e6..15b9f6849a 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/ExternalContactStateDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/ExternalContactStateDynamicsTest.cpp @@ -43,14 +43,14 @@ TEST_CASE("External Contact State Dynamics") // Create variable handler constexpr size_t sizeVariable = 6; VariablesHandler variableHandler; - REQUIRE(variableHandler.addVariable("ds", sizeVariable)); - REQUIRE(variableHandler.addVariable("tau_m", sizeVariable)); - REQUIRE(variableHandler.addVariable("tau_F", sizeVariable)); + REQUIRE(variableHandler.addVariable("JOINT_VELOCITIES", sizeVariable)); + REQUIRE(variableHandler.addVariable("MOTOR_TORQUES", sizeVariable)); + REQUIRE(variableHandler.addVariable("FRICTION_TORQUES", sizeVariable)); REQUIRE(variableHandler.addVariable("r_sole", 6)); ExternalContactStateDynamics r_sole_contact; - REQUIRE(r_sole_contact.initialize(parameterHandler)); + REQUIRE(r_sole_contact.initialize(parameterHandler, "r_sole")); REQUIRE(r_sole_contact.finalize(variableHandler)); Eigen::VectorXd currentState(covariance.size()); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp index d468e0e9ca..e633ba9493 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp @@ -116,10 +116,10 @@ IParametersHandler::shared_ptr createModelParameterHandler() void createUkfInput(VariablesHandler& stateVariableHandler, UKFInput& input) { - Eigen::VectorXd jointPos = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size); + Eigen::VectorXd jointPos = Eigen::VectorXd::Random(stateVariableHandler.getVariable("JOINT_VELOCITIES").size); input.robotJointPositions = jointPos; - Eigen::VectorXd jointAcc = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size); + Eigen::VectorXd jointAcc = Eigen::VectorXd::Random(stateVariableHandler.getVariable("JOINT_VELOCITIES").size); input.robotJointAccelerations = jointAcc; manif::SE3d basePose @@ -143,18 +143,18 @@ void createStateVector(UKFInput& input, { state.setZero(); - Eigen::VectorXd jointVel = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size); + Eigen::VectorXd jointVel = Eigen::VectorXd::Random(stateVariableHandler.getVariable("JOINT_VELOCITIES").size); - int offset = stateVariableHandler.getVariable("ds").offset; - int size = stateVariableHandler.getVariable("ds").size; + int offset = stateVariableHandler.getVariable("JOINT_VELOCITIES").offset; + int size = stateVariableHandler.getVariable("JOINT_VELOCITIES").size; for (int jointIndex = 0; jointIndex < size; jointIndex++) { state[offset + jointIndex] = jointVel(jointIndex); } // Compute joint torques from inverse dynamics on the full model - offset = stateVariableHandler.getVariable("tau_m").offset; - size = stateVariableHandler.getVariable("tau_m").size; + offset = stateVariableHandler.getVariable("MOTOR_TORQUES").offset; + size = stateVariableHandler.getVariable("MOTOR_TORQUES").size; iDynTree::LinkNetExternalWrenches extWrench(kinDyn->model()); extWrench.zero(); iDynTree::FreeFloatingGeneralizedTorques jointTorques(kinDyn->model()); @@ -174,7 +174,7 @@ IParametersHandler::shared_ptr createFrictionParameterHandler() // Create parameter handler auto parameterHandler = std::make_shared(); - const std::string name = "tau_F"; + const std::string name = "FRICTION_TORQUES"; Eigen::VectorXd covariance(6); covariance << 1e-3, 1e-3, 5e-3, 5e-3, 5e-3, 5e-3; const std::string model = "FrictionTorqueStateDynamics"; @@ -208,11 +208,11 @@ void computeTauFNext(UKFInput& input, IParametersHandler::shared_ptr frictionParamHandler, Eigen::Ref tauFNext) { - Eigen::VectorXd jointVel(stateVariableHandler.getVariable("ds").size); + Eigen::VectorXd jointVel(stateVariableHandler.getVariable("JOINT_VELOCITIES").size); - int offsetVel = stateVariableHandler.getVariable("ds").offset; + int offsetVel = stateVariableHandler.getVariable("JOINT_VELOCITIES").offset; - for (int jointIndex = 0; jointIndex < stateVariableHandler.getVariable("ds").size; jointIndex++) + for (int jointIndex = 0; jointIndex < stateVariableHandler.getVariable("JOINT_VELOCITIES").size; jointIndex++) { jointVel(jointIndex) = state[offsetVel + jointIndex]; } @@ -233,8 +233,8 @@ void computeTauFNext(UKFInput& input, std::chrono::nanoseconds dT; REQUIRE(frictionParamHandler->getParameter("sampling_time", dT)); - tauFNext = state.segment(stateVariableHandler.getVariable("tau_F").offset, - stateVariableHandler.getVariable("tau_F").size) + tauFNext = state.segment(stateVariableHandler.getVariable("FRICTION_TORQUES").offset, + stateVariableHandler.getVariable("FRICTION_TORQUES").size) + std::chrono::duration(dT).count() * dotTauF; } @@ -245,9 +245,9 @@ TEST_CASE("Friction Torque Dynamics") // Create variable handler constexpr size_t sizeVariable = 6; VariablesHandler stateVariableHandler; - REQUIRE(stateVariableHandler.addVariable("ds", sizeVariable)); - REQUIRE(stateVariableHandler.addVariable("tau_m", sizeVariable)); - REQUIRE(stateVariableHandler.addVariable("tau_F", sizeVariable)); + REQUIRE(stateVariableHandler.addVariable("JOINT_VELOCITIES", sizeVariable)); + REQUIRE(stateVariableHandler.addVariable("MOTOR_TORQUES", sizeVariable)); + REQUIRE(stateVariableHandler.addVariable("FRICTION_TORQUES", sizeVariable)); // Create model parameter handler auto modelParamHandler = createModelParameterHandler(); @@ -276,7 +276,7 @@ TEST_CASE("Friction Torque Dynamics") // Create friction torque dynamics FrictionTorqueStateDynamics tauFDynamics; REQUIRE(tauFDynamics.setSubModels(subModelList, kinDynWrapperList)); - REQUIRE(tauFDynamics.initialize(frictionParameterHandler)); + REQUIRE(tauFDynamics.initialize(frictionParameterHandler, "FRICTION_TORQUES")); REQUIRE(tauFDynamics.finalize(stateVariableHandler)); // Create an input for the ukf state diff --git a/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp index 6a1720ddf5..65cc8ddeda 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp @@ -126,10 +126,10 @@ IParametersHandler::shared_ptr createModelParameterHandler() void createUkfInput(VariablesHandler& stateVariableHandler, UKFInput& input) { - Eigen::VectorXd jointPos = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size); + Eigen::VectorXd jointPos = Eigen::VectorXd::Random(stateVariableHandler.getVariable("JOINT_VELOCITIES").size); input.robotJointPositions = jointPos; - Eigen::VectorXd jointAcc = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size); + Eigen::VectorXd jointAcc = Eigen::VectorXd::Random(stateVariableHandler.getVariable("JOINT_VELOCITIES").size); input.robotJointAccelerations = jointAcc; manif::SE3d basePose @@ -152,18 +152,18 @@ void createStateVector(UKFInput& input, { state.setZero(); - Eigen::VectorXd jointVel = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size); + Eigen::VectorXd jointVel = Eigen::VectorXd::Random(stateVariableHandler.getVariable("JOINT_VELOCITIES").size); - int offset = stateVariableHandler.getVariable("ds").offset; - int size = stateVariableHandler.getVariable("ds").size; + int offset = stateVariableHandler.getVariable("JOINT_VELOCITIES").offset; + int size = stateVariableHandler.getVariable("JOINT_VELOCITIES").size; for (int jointIndex = 0; jointIndex < size; jointIndex++) { state[offset + jointIndex] = jointVel(jointIndex); } // Compute joint torques from inverse dynamics on the full model - offset = stateVariableHandler.getVariable("tau_m").offset; - size = stateVariableHandler.getVariable("tau_m").size; + offset = stateVariableHandler.getVariable("MOTOR_TORQUES").offset; + size = stateVariableHandler.getVariable("MOTOR_TORQUES").size; iDynTree::LinkNetExternalWrenches extWrench(kinDyn->model()); extWrench.zero(); iDynTree::FreeFloatingGeneralizedTorques jointTorques(kinDyn->model()); @@ -193,8 +193,8 @@ void setRandomKinDynState(std::vector& subModelList, gravity.setZero(); gravity(2) = -BipedalLocomotion::Math::StandardAccelerationOfGravitation; - int offset = stateVariableHandler.getVariable("ds").offset; - int size = stateVariableHandler.getVariable("ds").size; + int offset = stateVariableHandler.getVariable("JOINT_VELOCITIES").offset; + int size = stateVariableHandler.getVariable("JOINT_VELOCITIES").size; Eigen::VectorXd jointVel(size); for (int jointIndex = 0; jointIndex < size; jointIndex++) @@ -226,8 +226,8 @@ void setRandomKinDynState(std::vector& subModelList, // Get sub-model joint velocities subModelJointVel[0].resize(subModelList[0].getModel().getNrOfDOFs()); - offset = stateVariableHandler.getVariable("ds").offset; - size = stateVariableHandler.getVariable("ds").size; + offset = stateVariableHandler.getVariable("JOINT_VELOCITIES").offset; + size = stateVariableHandler.getVariable("JOINT_VELOCITIES").size; for (int jointIdx = 0; jointIdx < subModelList[0].getModel().getNrOfDOFs(); jointIdx++) { subModelJointVel[0](jointIdx) = state[offset + subModelList[0].getJointMapping()[jointIdx]]; @@ -259,9 +259,9 @@ TEST_CASE("Gyroscope Measurement Dynamics") // Create state variable handler constexpr size_t sizeVariable = 6; VariablesHandler stateVariableHandler; - REQUIRE(stateVariableHandler.addVariable("ds", sizeVariable)); - REQUIRE(stateVariableHandler.addVariable("tau_m", sizeVariable)); - REQUIRE(stateVariableHandler.addVariable("tau_F", sizeVariable)); + REQUIRE(stateVariableHandler.addVariable("JOINT_VELOCITIES", sizeVariable)); + REQUIRE(stateVariableHandler.addVariable("MOTOR_TORQUES", sizeVariable)); + REQUIRE(stateVariableHandler.addVariable("FRICTION_TORQUES", sizeVariable)); REQUIRE(stateVariableHandler.addVariable("r_leg_ft_gyro_bias", 3)); // Create parameter handler to load the model @@ -292,7 +292,7 @@ TEST_CASE("Gyroscope Measurement Dynamics") // Create the dynamics GyroscopeMeasurementDynamics gyroDynamics; REQUIRE(gyroDynamics.setSubModels(subModelList, kinDynWrapperList)); - REQUIRE(gyroDynamics.initialize(gyroHandler)); + REQUIRE(gyroDynamics.initialize(gyroHandler, "r_leg_ft_gyro")); REQUIRE(gyroDynamics.finalize(stateVariableHandler)); // Create an input for the ukf state diff --git a/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp index 6270276c5b..ec60c0e83a 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp @@ -127,10 +127,10 @@ IParametersHandler::shared_ptr createModelParameterHandler() void createUkfInput(VariablesHandler& stateVariableHandler, UKFInput& input) { - Eigen::VectorXd jointPos = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size); + Eigen::VectorXd jointPos = Eigen::VectorXd::Random(stateVariableHandler.getVariable("JOINT_VELOCITIES").size); input.robotJointPositions = jointPos; - Eigen::VectorXd jointAcc = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size); + Eigen::VectorXd jointAcc = Eigen::VectorXd::Random(stateVariableHandler.getVariable("JOINT_VELOCITIES").size); input.robotJointAccelerations = jointAcc; manif::SE3d basePose @@ -153,18 +153,18 @@ void createStateVector(UKFInput& input, { state.setZero(); - Eigen::VectorXd jointVel = Eigen::VectorXd::Random(stateVariableHandler.getVariable("ds").size); + Eigen::VectorXd jointVel = Eigen::VectorXd::Random(stateVariableHandler.getVariable("JOINT_VELOCITIES").size); - int offset = stateVariableHandler.getVariable("ds").offset; - int size = stateVariableHandler.getVariable("ds").size; + int offset = stateVariableHandler.getVariable("JOINT_VELOCITIES").offset; + int size = stateVariableHandler.getVariable("JOINT_VELOCITIES").size; for (int jointIndex = 0; jointIndex < size; jointIndex++) { state[offset + jointIndex] = jointVel(jointIndex); } // Compute joint torques from inverse dynamics on the full model - offset = stateVariableHandler.getVariable("tau_m").offset; - size = stateVariableHandler.getVariable("tau_m").size; + offset = stateVariableHandler.getVariable("MOTOR_TORQUES").offset; + size = stateVariableHandler.getVariable("MOTOR_TORQUES").size; iDynTree::LinkNetExternalWrenches extWrench(kinDyn->model()); extWrench.zero(); iDynTree::FreeFloatingGeneralizedTorques jointTorques(kinDyn->model()); @@ -184,7 +184,7 @@ TEST_CASE("Joint Velocity State Dynamics") // Create parameter handler auto jointVelHandler = std::make_shared(); - const std::string name = "ds"; + const std::string name = "JOINT_VELOCITIES"; Eigen::VectorXd covariance(6); covariance << 1e-3, 1e-3, 5e-3, 5e-3, 5e-3, 5e-3; const std::string model = "JointVelocityStateDynamics"; @@ -200,9 +200,9 @@ TEST_CASE("Joint Velocity State Dynamics") // Create state variable handler constexpr size_t sizeVariable = 6; VariablesHandler stateVariableHandler; - REQUIRE(stateVariableHandler.addVariable("ds", sizeVariable)); - REQUIRE(stateVariableHandler.addVariable("tau_m", sizeVariable)); - REQUIRE(stateVariableHandler.addVariable("tau_F", sizeVariable)); + REQUIRE(stateVariableHandler.addVariable("JOINT_VELOCITIES", sizeVariable)); + REQUIRE(stateVariableHandler.addVariable("MOTOR_TORQUES", sizeVariable)); + REQUIRE(stateVariableHandler.addVariable("FRICTION_TORQUES", sizeVariable)); // Create parameter handler to load the model auto modelParamHandler = createModelParameterHandler(); @@ -232,7 +232,7 @@ TEST_CASE("Joint Velocity State Dynamics") // Create the dynamics JointVelocityStateDynamics dsDynamics; REQUIRE(dsDynamics.setSubModels(subModelList, kinDynWrapperList)); - REQUIRE(dsDynamics.initialize(jointVelHandler)); + REQUIRE(dsDynamics.initialize(jointVelHandler, "JOINT_VELOCITIES")); REQUIRE(dsDynamics.finalize(stateVariableHandler)); // Create an input for the ukf state @@ -252,8 +252,8 @@ TEST_CASE("Joint Velocity State Dynamics") REQUIRE(dsDynamics.update()); // Compute joint velocity at step k+1 by using the numerical integration - Eigen::VectorXd jointVel = state.segment(stateVariableHandler.getVariable("ds").offset, - stateVariableHandler.getVariable("ds").size); + Eigen::VectorXd jointVel = state.segment(stateVariableHandler.getVariable("JOINT_VELOCITIES").offset, + stateVariableHandler.getVariable("JOINT_VELOCITIES").size); jointVel = jointVel + std::chrono::duration(dT).count() * input.robotJointAccelerations; // Check the output diff --git a/src/Estimators/tests/RobotDynamicsEstimator/KinDynWrapperTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/KinDynWrapperTest.cpp index d4493f94c9..c6248aef78 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/KinDynWrapperTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/KinDynWrapperTest.cpp @@ -28,6 +28,7 @@ using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; using namespace BipedalLocomotion::ParametersHandler; using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion; void createModelLoader(IParametersHandler::shared_ptr group, iDynTree::ModelLoader& mdlLdr) { @@ -107,8 +108,22 @@ IParametersHandler::shared_ptr createModelParameterHandler() emptyGroupNamesFrames->setParameter("ukf_names", emptyVectorString); emptyGroupNamesFrames->setParameter("associated_joints", emptyVectorString); REQUIRE(modelParamHandler->setGroup("FT", emptyGroupNamesFrames)); - REQUIRE(modelParamHandler->setGroup("GYROSCOPE", emptyGroupNamesFrames)); - REQUIRE(modelParamHandler->setGroup("ACCELEROMETER", emptyGroupNamesFrames)); + auto gyroGroup = std::make_shared(); + std::vector gyroNameList = {"root_link"}; + std::vector gyroFrameList = {"root_link"}; + std::vector gyroUkfNameList = {"root_link_gyro"}; + gyroGroup->setParameter("names", gyroNameList); + gyroGroup->setParameter("frames", gyroFrameList); + gyroGroup->setParameter("ukf_names", gyroUkfNameList); + REQUIRE(modelParamHandler->setGroup("GYROSCOPE", gyroGroup)); + auto accGroup = std::make_shared(); + std::vector accNameList = {"root_link"}; + std::vector accFrameList = {"root_link"}; + std::vector accUkfNameList = {"root_link_acc"}; + accGroup->setParameter("names", accNameList); + accGroup->setParameter("frames", accFrameList); + accGroup->setParameter("ukf_names", accUkfNameList); + REQUIRE(modelParamHandler->setGroup("ACCELEROMETER", accGroup)); REQUIRE(modelParamHandler->setGroup("EXTERNAL_CONTACT", emptyGroupNamesFrames)); modelParamHandler->setParameter("joint_list", jointList); @@ -224,4 +239,75 @@ TEST_CASE("KinDynWrapper Test") REQUIRE(nuDot.head(6).isApprox(baseAcc.coeffs(), tolerance)); REQUIRE(nuDot.tail(numJoints).isApprox(jointAcc, tolerance)); + + // Test forward dynamics when written in terms of sensor proper acceleration + // Set the robot state with gravity equal to zero + baseAcc.coeffs().head(3) -= worldTBase.rotation().transpose() * gravity; + + gravity.setZero(); + REQUIRE(kinDyn->setRobotState(worldTBase.transform(), + jointPos, + iDynTree::make_span(baseVel.data(), manif::SE3d::Tangent::DoF), + jointVel, + gravity)); + + kinDyn->inverseDynamics(iDynTree::make_span(baseAcc.data(), manif::SE3d::Tangent::DoF), + jointAcc, + extWrench, + jointTorques); + + jointTrq = iDynTree::toEigen(jointTorques.jointTorques()); + + // 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 jointAccFD2(numJoints); + REQUIRE(kinDynWrapperList[0]->forwardDynamics(jointTrq, + Eigen::VectorXd::Zero(numJoints), + Eigen::VectorXd::Zero(numJoints), + baseAcc, + jointAccFD2)); + + REQUIRE(jointAcc.isApprox(jointAccFD2, tolerance)); + + + // Test forward dynamics with identity as base pose to verify that nothing depends on the base orientation + // Set the robot state with gravity equal to zero + worldTBase.setIdentity(); + + REQUIRE(kinDyn->setRobotState(worldTBase.transform(), + jointPos, + iDynTree::make_span(baseVel.data(), manif::SE3d::Tangent::DoF), + jointVel, + gravity)); + + kinDyn->inverseDynamics(iDynTree::make_span(baseAcc.data(), manif::SE3d::Tangent::DoF), + jointAcc, + extWrench, + jointTorques); + + jointTrq = iDynTree::toEigen(jointTorques.jointTorques()); + + // Set the sub-model state + kinDynWrapperList[0]->setRobotState(worldTBase.transform(), + jointPos, + iDynTree::make_span(baseVel.data(), + manif::SE3d::Tangent::DoF), + jointVel, + gravity); + + // Forward dynamics + REQUIRE(kinDynWrapperList[0]->forwardDynamics(jointTrq, + Eigen::VectorXd::Zero(numJoints), + Eigen::VectorXd::Zero(numJoints), + baseAcc, + jointAccFD2)); + + REQUIRE(jointAcc.isApprox(jointAccFD2, tolerance)); } diff --git a/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimationTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimationTest.cpp index 413eb3b2cb..7aadbb14e1 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimationTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimationTest.cpp @@ -60,9 +60,10 @@ struct Output }; // Struct to represent a sensor -struct SensorProperty { +struct SensorProperty +{ std::string sensorName; - std::string ukfSensorName; + std::string sensorFrame; }; ParametersHandler::IParametersHandler::shared_ptr loadConfiguration() @@ -103,15 +104,15 @@ loadSensors(std::weak_ptr handler) auto ftGroup = groupModel->getGroup("FT").lock(); REQUIRE(ftGroup != nullptr); std::unordered_map ftSensors; - std::vector names, ukfNames; + std::vector names, frames; REQUIRE(ftGroup->getParameter("names", names)); - REQUIRE(ftGroup->getParameter("ukf_names", ukfNames)); + REQUIRE(ftGroup->getParameter("frames", frames)); std::vector ftSensorPairs; for (int idx = 0; idx < names.size(); idx++) { SensorProperty temp; temp.sensorName = names[idx]; - temp.ukfSensorName = ukfNames[idx]; + temp.sensorFrame = frames[idx]; ftSensorPairs.emplace_back(temp); } sensors["ft"] = ftSensorPairs; @@ -120,13 +121,13 @@ loadSensors(std::weak_ptr handler) REQUIRE(contactGroup != nullptr); std::unordered_map contactSensors; REQUIRE(contactGroup->getParameter("names", names)); - REQUIRE(contactGroup->getParameter("ukf_names", ukfNames)); + REQUIRE(contactGroup->getParameter("frames", frames)); std::vector contactSensorPairs; for (int idx = 0; idx < names.size(); idx++) { SensorProperty temp; temp.sensorName = names[idx]; - temp.ukfSensorName = ukfNames[idx]; + temp.sensorFrame = frames[idx]; contactSensorPairs.emplace_back(temp); } sensors["contact"] = contactSensorPairs; @@ -135,13 +136,13 @@ loadSensors(std::weak_ptr handler) REQUIRE(accGroup != nullptr); std::unordered_map accSensors; REQUIRE(accGroup->getParameter("names", names)); - REQUIRE(accGroup->getParameter("ukf_names", ukfNames)); + REQUIRE(accGroup->getParameter("frames", frames)); std::vector accSensorPairs; for (int idx = 0; idx < names.size(); idx++) { SensorProperty temp; temp.sensorName = names[idx]; - temp.ukfSensorName = ukfNames[idx]; + temp.sensorFrame = frames[idx]; accSensorPairs.emplace_back(temp); } sensors["acc"] = accSensorPairs; @@ -150,13 +151,13 @@ loadSensors(std::weak_ptr handler) REQUIRE(gyroGroup != nullptr); std::unordered_map gyroSensors; REQUIRE(gyroGroup->getParameter("names", names)); - REQUIRE(gyroGroup->getParameter("ukf_names", ukfNames)); + REQUIRE(gyroGroup->getParameter("frames", frames)); std::vector gyroSensorPairs; for (int idx = 0; idx < names.size(); idx++) { SensorProperty temp; temp.sensorName = names[idx]; - temp.ukfSensorName = ukfNames[idx]; + temp.sensorFrame = frames[idx]; gyroSensorPairs.emplace_back(temp); } sensors["gyro"] = gyroSensorPairs; @@ -214,7 +215,6 @@ Dataset& loadData() // Unpack data static Dataset dataset; - log()->info("Unpacking data..."); auto temp = outStruct("s").asMultiDimensionalArray(); dataset.s = Conversions::toEigen(temp); @@ -247,14 +247,26 @@ Dataset& loadData() for (const auto& acc : outStruct3.fields()) { temp = outStruct3[acc].asMultiDimensionalArray(); - dataset.accs[acc] = Conversions::toEigen(temp); + if (acc == "base_imu_0") + { + dataset.accs[acc + "_acc"] = Conversions::toEigen(temp); + } else + { + dataset.accs[acc] = Conversions::toEigen(temp); + } } matioCpp::Struct outStruct4 = outStruct("gyros").asStruct(); for (const auto& gyro : outStruct4.fields()) { temp = outStruct4[gyro].asMultiDimensionalArray(); - dataset.gyros[gyro] = Conversions::toEigen(temp); + if (gyro == "base_imu_0") + { + dataset.gyros[gyro + "_gyro"] = Conversions::toEigen(temp); + } else + { + dataset.gyros[gyro] = Conversions::toEigen(temp); + } } return dataset; @@ -276,66 +288,73 @@ void createInitialState(Dataset& dataset, auto ftGroup = groupModel->getGroup("FT").lock(); REQUIRE(ftGroup != nullptr); - std::vector ftNames, ftUkfNames; + std::vector ftNames, ftFrames; REQUIRE(ftGroup->getParameter("names", ftNames)); - REQUIRE(ftGroup->getParameter("ukf_names", ftUkfNames)); - + REQUIRE(ftGroup->getParameter("frames", ftFrames)); for (int idx = 0; idx < ftNames.size(); idx++) { - output.ftWrenches[ftUkfNames[idx]] = dataset.fts[ftNames[idx]].row(0); - output.ftWrenchesBiases[ftUkfNames[idx] + "_bias"] = Eigen::VectorXd::Zero(6); + output.ftWrenches[ftNames[idx]] = dataset.fts[ftFrames[idx]].row(0); } auto contactGroup = groupModel->getGroup("EXTERNAL_CONTACT").lock(); REQUIRE(contactGroup != nullptr); - std::vector contactUkfNames; - REQUIRE(contactGroup->getParameter("ukf_names", contactUkfNames)); - for (int idx = 0; idx < contactUkfNames.size(); idx++) + std::vector contactNames; + REQUIRE(contactGroup->getParameter("names", contactNames)); + for (int idx = 0; idx < contactNames.size(); idx++) { - output.contactWrenches[contactUkfNames[idx]] = Eigen::VectorXd::Zero(6); + output.contactWrenches[contactNames[idx]] = Eigen::VectorXd::Zero(6); } auto accGroup = groupModel->getGroup("ACCELEROMETER").lock(); REQUIRE(accGroup != nullptr); - std::vector accUkfNames; - REQUIRE(accGroup->getParameter("ukf_names", accUkfNames)); - for (int idx = 0; idx < accUkfNames.size(); idx++) + std::vector accNames; + REQUIRE(accGroup->getParameter("names", accNames)); + for (int idx = 0; idx < accNames.size(); idx++) { - output.accelerometerBiases[accUkfNames[idx]+"_bias"] = Eigen::VectorXd::Zero(3); + output.linearAccelerations[accNames[idx]] = dataset.accs[accNames[idx]].row(0); } auto gyroGroup = groupModel->getGroup("GYROSCOPE").lock(); REQUIRE(gyroGroup != nullptr); - std::vector gyroUkfNames; - REQUIRE(gyroGroup->getParameter("ukf_names", gyroUkfNames)); - for (int idx = 0; idx < gyroUkfNames.size(); idx++) + std::vector gyroNames; + REQUIRE(gyroGroup->getParameter("names", gyroNames)); + for (int idx = 0; idx < gyroNames.size(); idx++) { - output.gyroscopeBiases[gyroUkfNames[idx]+"_bias"] = Eigen::VectorXd::Zero(3); + output.angularVelocities[gyroNames[idx]] = dataset.gyros[gyroNames[idx]].row(0); } } -void setInput(Dataset& dataset, int sample, RobotDynamicsEstimatorInput& input) +void setInput(Dataset& dataset, + int sample, + RobotDynamicsEstimatorInput& input, + std::unordered_map>& sensors) { + // Set input input.jointPositions = dataset.s.row(sample); input.jointVelocities = dataset.ds.row(sample); input.motorCurrents = dataset.im.row(sample); + input.frictionTorques.resize(input.motorCurrents.size()); + input.frictionTorques.setZero(); - for (auto const& [key, value] : dataset.fts) + for (int idx = 0; idx < sensors["ft"].size(); idx++) { - input.ftWrenches[key] = value.row(sample); + input.ftWrenches[sensors["ft"][idx].sensorName] + = dataset.fts[sensors["ft"][idx].sensorFrame].row(sample); } - for (auto const& [key, value] : dataset.accs) + for (int idx = 0; idx < sensors["acc"].size(); idx++) { - input.linearAccelerations[key] = value.row(sample); + input.linearAccelerations[sensors["acc"][idx].sensorName] + = dataset.accs[sensors["acc"][idx].sensorName].row(sample); } - for (auto const& [key, value] : dataset.gyros) + for (int idx = 0; idx < sensors["gyro"].size(); idx++) { - input.angularVelocities[key] = value.row(sample); + input.angularVelocities[sensors["gyro"][idx].sensorName] + = dataset.gyros[sensors["gyro"][idx].sensorName].row(sample); } } @@ -345,7 +364,8 @@ TEST_CASE("RobotDynamicsEstimator Test") auto parameterHandler = loadConfiguration(); // Save sensors - std::unordered_map> sensors = loadSensors(parameterHandler); + std::unordered_map> sensors + = loadSensors(parameterHandler); // Load robot model and create kindyn object SubModelCreator subModelCreator; @@ -375,7 +395,6 @@ TEST_CASE("RobotDynamicsEstimator Test") REQUIRE(estimator->setInitialState(output)); // Starting estimation - log()->info("Starting estimation..."); RobotDynamicsEstimatorInput input; @@ -389,28 +408,7 @@ TEST_CASE("RobotDynamicsEstimator Test") { int sample = sample_; - // Set input - input.jointPositions = dataset.s.row(sample); - input.jointVelocities = dataset.ds.row(sample); - input.motorCurrents = dataset.im.row(sample); - - for (int idx = 0; idx < sensors["ft"].size(); idx++) - { - input.ftWrenches[sensors["ft"][idx].ukfSensorName] - = dataset.fts[sensors["ft"][idx].sensorName].row(sample); - } - - for (int idx = 0; idx < sensors["acc"].size(); idx++) - { - input.linearAccelerations[sensors["acc"][idx].ukfSensorName] - = dataset.accs[sensors["acc"][idx].sensorName].row(sample); - } - - for (int idx = 0; idx < sensors["gyro"].size(); idx++) - { - input.angularVelocities[sensors["gyro"][idx].ukfSensorName] - = dataset.gyros[sensors["gyro"][idx].sensorName].row(sample); - } + setInput(dataset, sample, input, sensors); // Set input REQUIRE(estimator->setInput(input)); @@ -423,16 +421,27 @@ TEST_CASE("RobotDynamicsEstimator Test") // Check output REQUIRE((output.ds - dataset.ds.row(sample).transpose()).isZero(0.1)); - REQUIRE((output.tau_F - dataset.expectedTauF.row(sample).transpose()).isZero(0.1)); + for (int idx = 0; idx < output.tau_F.size(); idx++) + { + REQUIRE(std::abs(output.tau_F(idx) - dataset.expectedTauF.row(sample)(idx)) < 0.2); + } REQUIRE((output.tau_m - dataset.expectedTaum.row(sample).transpose()).isZero(0.1)); - REQUIRE(((output.tau_m - output.tau_F) - dataset.expectedTauj.row(sample).transpose()).isZero(0.1)); + for (int idx = 0; idx < output.tau_F.size(); idx++) + { + REQUIRE(std::abs((output.tau_m(idx) - output.tau_F(idx)) + - dataset.expectedTauj.row(sample)(idx)) + < 0.2); + } for (int idx = 0; idx < sensors["ft"].size(); idx++) { - REQUIRE(output.ftWrenches[sensors["ft"][idx].ukfSensorName].isApprox(dataset.fts[sensors["ft"][idx].sensorName].row(sample).transpose(), 0.1)); + REQUIRE( + output.ftWrenches[sensors["ft"][idx].sensorName] + .isApprox(dataset.fts[sensors["ft"][idx].sensorFrame].row(sample).transpose(), + 0.1)); } for (int idx = 0; idx < sensors["contact"].size(); idx++) { - REQUIRE((output.contactWrenches[sensors["contact"][idx].ukfSensorName]).isZero(0.1)); + REQUIRE((output.contactWrenches[sensors["contact"][idx].sensorFrame]).isZero(0.1)); } } } diff --git a/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityStateDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityStateDynamicsTest.cpp index bf29d73ddc..01a3ad1cfc 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityStateDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityStateDynamicsTest.cpp @@ -37,16 +37,16 @@ TEST_CASE("Zero Velocity Dynamics") // Create variable handler constexpr size_t sizeVariable = 6; VariablesHandler variableHandler; - REQUIRE(variableHandler.addVariable("ds", sizeVariable)); - REQUIRE(variableHandler.addVariable("tau_m", sizeVariable)); - REQUIRE(variableHandler.addVariable("tau_F", sizeVariable)); + REQUIRE(variableHandler.addVariable("JOINT_VELOCITIES", sizeVariable)); + REQUIRE(variableHandler.addVariable("MOTOR_TORQUES", sizeVariable)); + REQUIRE(variableHandler.addVariable("FRICTION_TORQUES", sizeVariable)); REQUIRE(variableHandler.addVariable("r_leg_ft", sizeVariable)); REQUIRE(variableHandler.addVariable("r_leg_ft_bias", sizeVariable)); REQUIRE(variableHandler.addVariable("r_foot_front_ft", sizeVariable)); ZeroVelocityStateDynamics tau_m; - REQUIRE(tau_m.initialize(parameterHandler)); + REQUIRE(tau_m.initialize(parameterHandler, "MOTOR_TORQUES")); REQUIRE(tau_m.finalize(variableHandler)); Eigen::VectorXd currentState(sizeVariable); @@ -57,7 +57,7 @@ TEST_CASE("Zero Velocity Dynamics") Eigen::VectorXd state(sizeVariable * variableHandler.getNumberOfVariables()); state.setZero(); - state.segment(variableHandler.getVariable("tau_m").offset, variableHandler.getVariable("tau_m").size) = currentState; + state.segment(variableHandler.getVariable("MOTOR_TORQUES").offset, variableHandler.getVariable("MOTOR_TORQUES").size) = currentState; tau_m.setState(state); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/model.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/model.ini index f2f227541e..ef69dc09c5 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/config/model.ini +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/model.ini @@ -2,22 +2,18 @@ joint_list ("joint_1", "joint_2", "joint_3", "joint_4") base_link "base_link" [EXTERNAL_CONTACT] -names ("ft_1") +names ("ft_1_contact") frames ("ft_1") -ukf_names ("ft_1_contact") [FT] -names ("ft_1", "ft_2") +names ("ft_1_ft", "ft_2_ft") frames ("ft_1", "ft_2") associated_joints ("ft_1", "ft_2") -ukf_names ("ft_1_ft", "ft_2_ft") [ACCELEROMETER] -names ("ft_1_acc", "ft_2_acc") -frames ("ft_1", "ft_2") -ukf_names ("ft_1_acc", "ft_2_acc") +names ("base_imu_0_acc", "ft_1_acc", "ft_2_acc") +frames ("base_imu_0", "ft_1", "ft_2") [GYROSCOPE] -names ("ft_1_gyro", "ft_2_gyro") -frames ("ft_1", "ft_2") -ukf_names ("ft_1_gyro", "ft_2_gyro") +names ("base_imu_0_gyro", "ft_1_gyro", "ft_2_gyro") +frames ("base_imu_0", "ft_1", "ft_2") diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini index 011c1ca27c..bd99a27fa1 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini @@ -1,58 +1,109 @@ -dynamics_list ("JOINT_VELOCITY", "MOTOR_CURRENT", "FT_1", "FT_2", "FT_1_ACC", "FT_2_ACC", "FT_1_GYRO", "FT_2_GYRO") +dynamics_list ("JOINT_VELOCITIES", "MOTOR_CURRENTS", "FRICTION_TORQUES", + "BASE_ACC", "BASE_GYRO", + "FT_1", "FT_1_ACC_1", "FT_1_ACC_2", "FT_1_GYRO_1", "FT_1_GYRO_2", + "FT_2", "FT_2_ACC_1", "FT_2_ACC_2", "FT_2_GYRO_1", "FT_2_GYRO_2") -[JOINT_VELOCITY] -name "ds" -elements ("joint_1", "joint_2", "joint_3", "joint_4") +[JOINT_VELOCITIES] +input_name "ds" +associated_state "JOINT_VELOCITIES" covariance (1e-8, 1e-8, 1e-8, 1e-8) use_bias 0 dynamic_model "ConstantMeasurementModel" -[MOTOR_CURRENT] -name "i_m" -elements ("joint_1", "joint_2", "joint_3", "joint_4") +[MOTOR_CURRENTS] +input_name "i_m" covariance (1e-8, 1e-8, 1e-8, 1e-8) gear_ratio (100.0, 100.0, 100.0, 100.0) torque_constant (0.047, 0.047, 0.047, 0.047) dynamic_model "MotorCurrentMeasurementDynamics" +[FRICTION_TORQUES] +input_name "tau_F" +associated_state "FRICTION_TORQUES" +covariance (1e-6, 1e-6, 1e-6, 1e-6) +use_bias 0 +dynamic_model "ConstantMeasurementModel" + [FT_1] -name "ft_1_ft" -elements ("fx", "fy", "fz", "mx", "my", "mz") +input_name "ft_1_ft" +associated_state "FT_1" covariance (1e-8, 1e-8, 1e-8, 1e-8, 1e-8, 1e-8) use_bias 0 dynamic_model "ConstantMeasurementModel" [FT_2] -name "ft_2_ft" -elements ("fx", "fy", "fz", "mx", "my", "mz") +input_name "ft_2_ft" +associated_state "FT_2" covariance (1e-8, 1e-8, 1e-8, 1e-8, 1e-8, 1e-8) use_bias 0 dynamic_model "ConstantMeasurementModel" -[FT_1_ACC] -name "ft_1_acc" -elements ("x", "y", "z") +[BASE_ACC] +input_name "base_imu_0_acc" +associated_state "BASE_ACC" +covariance (1e-3, 1e-3, 1e-3) +use_bias 0 +dynamic_model "ConstantMeasurementModel" + +[FT_1_ACC_1] +input_name "ft_1_acc" +associated_state "FT_1_ACC" +covariance (1e-3, 1e-3, 1e-3) +use_bias 0 +dynamic_model "ConstantMeasurementModel" + +[FT_1_ACC_2] +input_name "ft_1_acc" +associated_state "FT_1_ACC" covariance (1e-3, 1e-3, 1e-3) use_bias 0 dynamic_model "AccelerometerMeasurementDynamics" -[FT_2_ACC] -name "ft_2_acc" -elements ("x", "y", "z") +[FT_2_ACC_1] +input_name "ft_2_acc" +associated_state "FT_2_ACC" +covariance (1e-3, 1e-3, 1e-3) +use_bias 0 +dynamic_model "ConstantMeasurementModel" + +[FT_2_ACC_2] +input_name "ft_2_acc" +associated_state "FT_2_ACC" covariance (1e-3, 1e-3, 1e-3) use_bias 0 dynamic_model "AccelerometerMeasurementDynamics" -[FT_1_GYRO] -name "ft_1_gyro" -elements ("x", "y", "z") +[BASE_GYRO] +input_name "base_imu_0_gyro" +associated_state "BASE_GYRO" +covariance (1e-3, 1e-3, 1e-3) +use_bias 0 +dynamic_model "ConstantMeasurementModel" + +[FT_1_GYRO_1] +input_name "ft_1_gyro" +associated_state "FT_1_GYRO" +covariance (1e-3, 1e-3, 1e-3) +use_bias 0 +dynamic_model "ConstantMeasurementModel" + +[FT_1_GYRO_2] +input_name "ft_1_gyro" +associated_state "FT_1_GYRO" covariance (1e-3, 1e-3, 1e-3) use_bias 0 dynamic_model "GyroscopeMeasurementDynamics" -[FT_2_GYRO] -name "ft_2_gyro" -elements ("x", "y", "z") +[FT_2_GYRO_1] +input_name "ft_2_gyro" +associated_state "FT_2_GYRO" +covariance (1e-3, 1e-3, 1e-3) +use_bias 0 +dynamic_model "ConstantMeasurementModel" + +[FT_2_GYRO_2] +input_name "ft_2_gyro" +associated_state "FT_2_GYRO" covariance (1e-3, 1e-3, 1e-3) use_bias 0 dynamic_model "GyroscopeMeasurementDynamics" diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini index fa78c9ad1b..6bc12d016b 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini @@ -1,47 +1,76 @@ -dynamics_list ("JOINT_VELOCITY", "MOTOR_TORQUE", "FRICTION_TORQUE", "FT_1", "FT_2", "CONTACT_1") +dynamics_list ("JOINT_VELOCITIES", "MOTOR_TORQUES", "FRICTION_TORQUES", + "BASE_ACC", "BASE_GYRO", "FT_1", "FT_1_ACC", "FT_1_GYRO", "CONTACT_1", + "FT_2", "FT_2_ACC", "FT_2_GYRO") -[JOINT_VELOCITY] -name "ds" -elements ("joint_1", "joint_2", "joint_3", "joint_4") +[JOINT_VELOCITIES] +input_name "ds" covariance (1e-4, 1e-4, 1e-4, 1e-4) initial_covariance (1e-2, 1e-2, 1e-2, 1e-2) dynamic_model "JointVelocityStateDynamics" -[MOTOR_TORQUE] -name "tau_m" -elements ("joint_1", "joint_2", "joint_3", "joint_4") +[MOTOR_TORQUES] +input_name "tau_m" covariance (1e-2, 1e-2, 1e-2, 1e-2) initial_covariance (1e-2, 1e-2, 1e-2, 1e-2) dynamic_model "ZeroVelocityStateDynamics" -[FRICTION_TORQUE] -name "tau_F" -elements ("joint_1", "joint_2", "joint_3", "joint_4") -covariance (1e-2, 1e-2, 1e-2, 1e-2) +[FRICTION_TORQUES] +input_name "tau_F" +covariance (1e-3, 1e-3, 1e-3, 1e-3) initial_covariance (1e-2, 1e-2, 1e-2, 1e-2) -dynamic_model "FrictionTorqueStateDynamics" -friction_k0 (0.1, 0.1, 0.1, 0.1) -friction_k1 (0.1, 0.1, 0.1, 0.1) -friction_k2 (0.1, 0.1, 0.1, 0.1) +dynamic_model "ZeroVelocityStateDynamics" [FT_1] -name "ft_1_ft" -elements ("fx", "fy", "fz", "mx", "my", "mz") +input_name "ft_1_ft" covariance (1e-1, 1e-1, 1e-1, 1e-3, 1e-3, 1e-3) initial_covariance (1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2) dynamic_model "ZeroVelocityStateDynamics" [FT_2] -name "ft_2_ft" -elements ("fx", "fy", "fz", "mx", "my", "mz") +input_name "ft_2_ft" covariance (1e-1, 1e-1, 1e-1, 1e-3, 1e-3, 1e-3) initial_covariance (1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2) dynamic_model "ZeroVelocityStateDynamics" [CONTACT_1] -name "ft_1_contact" -elements ("fx", "fy", "fz", "mx", "my", "mz") +input_name "ft_1_contact" covariance (1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2) initial_covariance (1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2) -k (1e-1, 1e-1, 1e-1, 1e-1, 1e-1, 1e-1) +k (1e2, 1e2, 1e2, 1e2, 1e2, 1e2) +dynamic_model "ExternalContactStateDynamics" + +[BASE_ACC] +input_name "base_imu_0_acc" +covariance (1e-3, 1e-3, 1e-3) +initial_covariance (1e-2, 1e-2, 1e-2) +dynamic_model "ZeroVelocityStateDynamics" + +[FT_1_ACC] +input_name "ft_1_acc" +covariance (1e-3, 1e-3, 1e-3) +initial_covariance (1e-2, 1e-2, 1e-2) +dynamic_model "ZeroVelocityStateDynamics" + +[FT_2_ACC] +input_name "ft_2_acc" +covariance (1e-3, 1e-3, 1e-3) +initial_covariance (1e-2, 1e-2, 1e-2) +dynamic_model "ZeroVelocityStateDynamics" + +[BASE_GYRO] +input_name "base_imu_0_gyro" +covariance (1e-3, 1e-3, 1e-3) +initial_covariance (1e-2, 1e-2, 1e-2) +dynamic_model "ZeroVelocityStateDynamics" + +[FT_1_GYRO] +input_name "ft_1_gyro" +covariance (1e-3, 1e-3, 1e-3) +initial_covariance (1e-2, 1e-2, 1e-2) +dynamic_model "ZeroVelocityStateDynamics" + +[FT_2_GYRO] +input_name "ft_2_gyro" +covariance (1e-3, 1e-3, 1e-3) +initial_covariance (1e-2, 1e-2, 1e-2) dynamic_model "ZeroVelocityStateDynamics"