Add accelerometer and gyroscope dynamic models

CHANGELOG.md

@@ -6,6 +6,7 @@ All notable changes to this project are documented in this file.
 - Implement a class to perform inference with the [MANN network](https://github.com/ami-iit/mann-pytorch) (https://github.com/ami-iit/bipedal-locomotion-framework/pull/652)
 - Add some useful methods to the `SubModel` and `SubModelKinDynWrapper` classes (https://github.com/ami-iit/bipedal-locomotion-framework/pull/661)
 - Implement `Dynamics`, `ZeroVelocityDynamics`, and `JointVelocityStateDynamics` classes in `RobotDynamicsEstimator` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/662)
+- Implement `AccelerometerMeasurementDynamics` and `GyroscopeMeasurementDynamics` classes in `RobotDynamicsEstimator` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/666)
 
 ### Changed
 

src/Estimators/CMakeLists.txt

@@ -28,10 +28,11 @@ if(FRAMEWORK_COMPILE_RobotDynamicsEstimator)
  add_bipedal_locomotion_library(
  NAME RobotDynamicsEstimator
  SOURCES src/SubModel.cpp src/SubModelKinDynWrapper.cpp src/Dynamics.cpp src/ZeroVelocityStateDynamics.cpp src/JointVelocityStateDynamics.cpp
- src/ConstantMeasurementModel.cpp
+ src/ConstantMeasurementModel.cpp src/AccelerometerMeasurementDynamics.cpp src/GyroscopeMeasurementDynamics.cpp
  SUBDIRECTORIES tests/RobotDynamicsEstimator
  PUBLIC_HEADERS ${H_PREFIX}/SubModel.h ${H_PREFIX}/SubModelKinDynWrapper.h ${H_PREFIX}/Dynamics.h ${H_PREFIX}/ZeroVelocityStateDynamics.h
- ${H_PREFIX}/JointVelocityStateDynamics.h ${H_PREFIX}/ConstantMeasurementModel.h
+ ${H_PREFIX}/JointVelocityStateDynamics.h ${H_PREFIX}/ConstantMeasurementModel.h ${H_PREFIX}/AccelerometerMeasurementDynamics.h
+ ${H_PREFIX}/GyroscopeMeasurementDynamics.h
  PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler MANIF::manif BipedalLocomotion::System BipedalLocomotion::Math iDynTree::idyntree-high-level
  iDynTree::idyntree-core iDynTree::idyntree-model iDynTree::idyntree-modelio Eigen3::Eigen
  PRIVATE_LINK_LIBRARIES BipedalLocomotion::TextLogging BipedalLocomotion::ManifConversions

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

@@ -0,0 +1,147 @@
+/**
+ * @file AccelerometerMeasurementDynamics.h
+ * @authors Ines Sorrentino
+ * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_ACCELEROMETER_MEASUREMENT_DYNAMICS_H
+#define BIPEDAL_LOCOMOTION_ESTIMATORS_ACCELEROMETER_MEASUREMENT_DYNAMICS_H
+
+#include <BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h>
+#include <memory>
+
+namespace BipedalLocomotion
+{
+namespace Estimators
+{
+namespace RobotDynamicsEstimator
+{
+
+/**
+ * The AccelerometerMeasurementDynamics class is a concrete implementation of the Dynamics.
+ * Please use this element if you want to use the model dynamics of an accelerometer defined,
+ * using the kinematics, as the time derivative of the frame linear velocity:
+ * \f[
+ * v = J \nu
+ * \f]
+ * The AccelerometerMeasurementDynamics represents the following equation in the continuous time:
+ * \f[
+ * \dot{v}^{accelerometer} = \dot{J} \nu + J \dot{\nu} = \dot{J} \nu + J^{base} \dot{v}^{base} +
+ * J^{joints} \ddot{s} \f] where the joint acceleration is given by the forward dynamics equation.
+ */
+class AccelerometerMeasurementDynamics : public Dynamics
+{
+ bool m_useBias{false}; /**< If true the dynamics depends on a bias additively. */
+ Eigen::VectorXd m_bias; /**< The bias is initialized and used only if m_useBias is true. False
+ if not specified. */
+ std::string m_biasVariableName; /**< Name of the variable containing the bias in the variable
+ handler. */
+ std::vector<SubModel> m_subModelList; /** List of SubModel objects. */
+ std::vector<std::shared_ptr<SubModelKinDynWrapper>> m_kinDynWrapperList; /**< List of pointers
+ to
+ SubModelKinDynWrapper
+ objects. */
+ bool m_isSubModelListSet{false}; /**< Boolean flag saying if the sub-model list has been set. */
+ std::vector<Eigen::VectorXd> m_subModelJointAcc; /**< Updated joint acceleration of each
+ sub-model. */
+ Eigen::Vector3d m_gravity; /**< Gravitational acceleration. */
+ std::vector<std::size_t> m_subModelsWithAccelerometer; /**< List of indeces saying which
+ sub-model in the m_subDynamics list
+ containa the accelerometer. */
+ UKFInput m_ukfInput; /**< Input of the UKF used to update the dynamics. */
+ std::string m_name; /**< Name of dynamics. */
+ System::VariablesHandler m_stateVariableHandler; /**< Variable handler describing the variables
+ and the sizes in the ukf state vector. */
+ Eigen::VectorXd m_covSingleVar; /**< Covariance of the accelerometer measurement from
+ configuration. */
+ Eigen::VectorXd m_JdotNu; /**< Jdot nu. */
+ Eigen::VectorXd m_JvdotBase; /**< Jacobian times the base acceleration. */
+ Eigen::VectorXd m_Jsdotdot; /**< Jacobian times the joint acceleration. */
+ Eigen::Vector3d m_accRg; /**< Gravity rotated in the accelerometer frame. */
+ Eigen::Vector3d m_vCrossW; /**< Accelerometer linear velocity cross accelerometer angular
+ velocity. */
+ Eigen::Vector3d m_linVel; /**< Accelerometer linear velocity. */
+ Eigen::Vector3d m_angVel; /**< Accelerometer angular velocity. */
+
+public:
+ /*
+ * Constructor
+ */
+ AccelerometerMeasurementDynamics();
+
+ /*
+ * Destructor
+ */
+ virtual ~AccelerometerMeasurementDynamics();
+
+ /**
+ * Initialize the state dynamics.
+ * @param paramHandler pointer to the parameters handler.
+ * @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 |
+ * | `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<const ParametersHandler::IParametersHandler> paramHandler) override;
+
+ /**
+ * Finalize the Dynamics.
+ * @param stateVariableHandler object describing the variables in the state vector.
+ * @note You should call this method after you add ALL the state dynamics to the state variable
+ * handler.
+ * @return true in case of success, false otherwise.
+ */
+ bool finalize(const System::VariablesHandler& stateVariableHandler) override;
+
+ /**
+ * Set the SubModelKinDynWrapper object.
+ * @param subModelList list of SubModel objects
+ * @param kinDynWrapperList list of pointers to SubModelKinDynWrapper objects.
+ * @return True in case of success, false otherwise.
+ */
+ bool setSubModels(
+ const std::vector<SubModel>& subModelList,
+ const std::vector<std::shared_ptr<SubModelKinDynWrapper>>& kinDynWrapperList) override;
+
+ /**
+ * Controls whether the variable handler contains the variables on which the dynamics depend.
+ * @return True in case all the dependencies are contained in the variable handler, false
+ * otherwise.
+ */
+ bool checkStateVariableHandler() override;
+
+ /**
+ * Update the content of the element.
+ * @return True in case of success, false otherwise.
+ */
+ bool update() override;
+
+ /**
+ * Set the state of the ukf needed to update the dynamics of the measurement variable associated
+ * to ths object.
+ * @param ukfState reference to the ukf state.
+ */
+ void setState(const Eigen::Ref<const Eigen::VectorXd> ukfState) override;
+
+ /**
+ * Set a `UKFInput` object.
+ * @param ukfInput reference to the UKFInput struct.
+ */
+ void setInput(const UKFInput& ukfInput) override;
+
+}; // class AccelerometerMeasurementDynamics
+
+BLF_REGISTER_UKF_DYNAMICS(AccelerometerMeasurementDynamics,
+ ::BipedalLocomotion::Estimators::RobotDynamicsEstimator::Dynamics);
+
+} // namespace RobotDynamicsEstimator
+} // namespace Estimators
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_ACCELEROMETER_MEASUREMENT_DYNAMICS_H

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

@@ -0,0 +1,140 @@
+/**
+ * @file GyroscopeMeasurementDynamics.h
+ * @authors Ines Sorrentino
+ * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_ACCELEROMETER_MEASUREMENT_DYNAMICS_H
+#define BIPEDAL_LOCOMOTION_ESTIMATORS_ACCELEROMETER_MEASUREMENT_DYNAMICS_H
+
+#include <memory>
+#include <BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h>
+
+namespace BipedalLocomotion
+{
+namespace Estimators
+{
+namespace RobotDynamicsEstimator
+{
+
+/**
+ * The GyroscopeMeasurementDynamics class is a concrete implementation of the Dynamics.
+ * Please use this element if you want to use the model dynamics of a gyroscope.
+ * The GyroscopeMeasurementDynamics represents the following equation in the continuous time:
+ * \f[
+ * \omega^{gyroscope} = J \nu = J^{base} v^{base} + J^{joints} \dot{s}
+ * \f]
+ * where the joint velocity is estimated by the ukf.
+ */
+class GyroscopeMeasurementDynamics : public Dynamics
+{
+ bool m_useBias{false}; /**< If true the dynamics depends on a bias additively. */
+ Eigen::VectorXd m_bias; /**< The bias is initialized and used only if m_useBias is true. False
+ if not specified. */
+ std::string m_biasVariableName; /**< Name of the variable containing the bias in the variable
+ handler. */
+ bool m_isSubModelListSet{false}; /**< Boolean flag saying if the sub-model list has been set. */
+ int m_nrOfSubDynamics; /**< Number of sub-dynamics which corresponds to the number of
+ sub-models. */
+ std::vector<std::shared_ptr<SubModelKinDynWrapper>> m_subModelKinDynList; /**< Vector of
+ SubModelKinDynWrapper
+ objects. */
+ std::vector<SubModel> m_subModelList; /**< Vector of SubModel objects. */
+ std::vector<Eigen::VectorXd> m_subModelJointVel; /**< Updated joint velocities of each
+ sub-model. */
+ std::vector<std::size_t> m_subModelWithGyro; /**< List of indeces saying which sub-model in the
+ m_subDynamics list containa the gyroscope. */
+ Eigen::VectorXd m_jointVelocityFullModel; /**< Vector of joint velocities. */
+ UKFInput m_ukfInput; /**< Input of the UKF used to update the dynamics. */
+ std::string m_name; /**< Name of dynamics. */
+ std::vector<std::string> m_elements = {}; /**< Elements composing the variable vector. */
+ System::VariablesHandler m_stateVariableHandler; /**< Variable handler describing the variables
+ and the sizes in the ukf state vector. */
+ Eigen::VectorXd m_covSingleVar; /**< Covariance of the gyroscope measurement from configuration.
+ */
+ manif::SE3d::Tangent m_subModelBaseVel; /**< Submodel base velocity. */
+ Eigen::VectorXd m_JvBase; /**< Jacobian times base velocity. */
+ Eigen::VectorXd m_Jsdot; /**< Jacobian times joint velocity. */
+
+public:
+ /*
+ * Constructor
+ */
+ GyroscopeMeasurementDynamics();
+
+ /*
+ * Destructor
+ */
+ virtual ~GyroscopeMeasurementDynamics();
+
+ /**
+ * Initialize the state dynamics.
+ * @param paramHandler pointer to the parameters handler.
+ * @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 |
+ * | `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<const ParametersHandler::IParametersHandler> paramHandler) override;
+
+ /**
+ * Finalize the Dynamics.
+ * @param stateVariableHandler object describing the variables in the state vector.
+ * @note You should call this method after you add ALL the state dynamics to the state variable
+ * handler.
+ * @return true in case of success, false otherwise.
+ */
+ bool finalize(const System::VariablesHandler& stateVariableHandler) override;
+
+ /**
+ * Set the SubModelKinDynWrapper object.
+ * @param subModelList list of SubModel objects
+ * @param kinDynWrapperList list of pointers to SubModelKinDynWrapper objects.
+ * @return True in case of success, false otherwise.
+ */
+ bool setSubModels(
+ const std::vector<SubModel>& subModelList,
+ const std::vector<std::shared_ptr<SubModelKinDynWrapper>>& kinDynWrapperList) override;
+
+ /**
+ * Controls whether the variable handler contains the variables on which the dynamics depend.
+ * @return True in case all the dependencies are contained in the variable handler, false
+ * otherwise.
+ */
+ bool checkStateVariableHandler() override;
+
+ /**
+ * Update the content of the element.
+ * @return True in case of success, false otherwise.
+ */
+ bool update() override;
+
+ /**
+ * Set the state of the ukf needed to update the dynamics of the measurement variable associated
+ * to ths object.
+ * @param ukfState reference to the ukf state.
+ */
+ void setState(const Eigen::Ref<const Eigen::VectorXd> ukfState) override;
+
+ /**
+ * Set a `UKFInput` object.
+ * @param ukfInput reference to the UKFInput struct.
+ */
+ void setInput(const UKFInput& ukfInput) override;
+
+}; // class GyroscopeMeasurementDynamics
+
+BLF_REGISTER_UKF_DYNAMICS(GyroscopeMeasurementDynamics,
+ ::BipedalLocomotion::Estimators::RobotDynamicsEstimator::Dynamics);
+
+} // namespace RobotDynamicsEstimator
+} // namespace Estimators
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_ACCELEROMETER_MEASUREMENT_DYNAMICS_H