Add process model for friction torque and measurement model for motor current

CHANGELOG.md

@@ -7,6 +7,7 @@ All notable changes to this project are documented in this file.
 - 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)
+- Implement `FrictionTorqueStateDynamics` and `MotorCurrentMeasurementDynamics` classes in `RobotDynamicsEstimator` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/667)
 
 ### Changed
 

src/Estimators/CMakeLists.txt

@@ -3,38 +3,39 @@
 # BSD-3-Clause license.
 
 if(FRAMEWORK_COMPILE_Estimators)
- set(H_PREFIX include/BipedalLocomotion/Estimators)
- add_bipedal_locomotion_library(
- NAME Estimators
- SOURCES src/RecursiveLeastSquare.cpp
- PUBLIC_HEADERS ${H_PREFIX}/RecursiveLeastSquare.h
- SUBDIRECTORIES tests/Estimators
- PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler BipedalLocomotion::System Eigen3::Eigen)
+  set(H_PREFIX include/BipedalLocomotion/Estimators)
+  add_bipedal_locomotion_library(
+  NAME Estimators
+  SOURCES src/RecursiveLeastSquare.cpp
+  PUBLIC_HEADERS ${H_PREFIX}/RecursiveLeastSquare.h
+  SUBDIRECTORIES tests/Estimators
+  PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler BipedalLocomotion::System Eigen3::Eigen)
 endif()
 
 if(FRAMEWORK_COMPILE_FloatingBaseEstimators)
- set(H_PREFIX include/BipedalLocomotion/FloatingBaseEstimators)
- add_bipedal_locomotion_library(
- NAME FloatingBaseEstimators
- SOURCES src/FloatingBaseEstimator.cpp src/InvariantEKFBaseEstimator.cpp src/LeggedOdometry.cpp
- SUBDIRECTORIES tests/FloatingBaseEstimators
- PUBLIC_HEADERS ${H_PREFIX}/FloatingBaseEstimatorParams.h ${H_PREFIX}/FloatingBaseEstimatorIO.h ${H_PREFIX}/FloatingBaseEstimator.h ${H_PREFIX}/InvariantEKFBaseEstimator.h ${H_PREFIX}/LeggedOdometry.h
- PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler BipedalLocomotion::ManifConversions iDynTree::idyntree-high-level iDynTree::idyntree-core iDynTree::idyntree-model BipedalLocomotion::System Eigen3::Eigen BipedalLocomotion::Contacts iDynTree::idyntree-modelio BipedalLocomotion::TextLogging
- PRIVATE_LINK_LIBRARIES MANIF::manif)
+  set(H_PREFIX include/BipedalLocomotion/FloatingBaseEstimators)
+  add_bipedal_locomotion_library(
+  NAME FloatingBaseEstimators
+  SOURCES src/FloatingBaseEstimator.cpp src/InvariantEKFBaseEstimator.cpp src/LeggedOdometry.cpp
+  SUBDIRECTORIES tests/FloatingBaseEstimators
+  PUBLIC_HEADERS ${H_PREFIX}/FloatingBaseEstimatorParams.h ${H_PREFIX}/FloatingBaseEstimatorIO.h ${H_PREFIX}/FloatingBaseEstimator.h ${H_PREFIX}/InvariantEKFBaseEstimator.h ${H_PREFIX}/LeggedOdometry.h
+  PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler BipedalLocomotion::ManifConversions iDynTree::idyntree-high-level iDynTree::idyntree-core iDynTree::idyntree-model BipedalLocomotion::System Eigen3::Eigen BipedalLocomotion::Contacts iDynTree::idyntree-modelio BipedalLocomotion::TextLogging
+  PRIVATE_LINK_LIBRARIES MANIF::manif)
 endif()
 
 if(FRAMEWORK_COMPILE_RobotDynamicsEstimator)
- set(H_PREFIX include/BipedalLocomotion/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/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}/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
-)
+ set(H_PREFIX include/BipedalLocomotion/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/AccelerometerMeasurementDynamics.cpp src/GyroscopeMeasurementDynamics.cpp
+ src/FrictionTorqueStateDynamics.cpp src/MotorCurrentMeasurementDynamics.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}/AccelerometerMeasurementDynamics.h
+ ${H_PREFIX}/GyroscopeMeasurementDynamics.h ${H_PREFIX}/FrictionTorqueStateDynamics.h ${H_PREFIX}/MotorCurrentMeasurementDynamics.h
+ PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler MANIF::manif BipedalLocomotion::System BipedalLocomotion::Math iDynTree::idyntree-high-level
+ iDynTree::idyntree-core iDynTree::idyntree-model iDynTree::idyntree-modelio Eigen3::Eigen
+ PRIVATE_LINK_LIBRARIES BipedalLocomotion::TextLogging BipedalLocomotion::ManifConversions
+ )
 endif()

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

@@ -0,0 +1,137 @@
+/**
+ * @file FrictionTorqueStateDynamics.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_FRICTION_TORQUE_STATE_DYNAMICS_H
+#define BIPEDAL_LOCOMOTION_ESTIMATORS_FRICTION_TORQUE_STATE_DYNAMICS_H
+
+#include <BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h>
+#include <memory>
+
+namespace BipedalLocomotion
+{
+namespace Estimators
+{
+namespace RobotDynamicsEstimator
+{
+
+/**
+ * The FrictionTorqueDynamics class is a concrete implementation of the Dynamics.
+ * Please use this element if you want to use a specific friction torque model dynamics.
+ * The FrictionTorqueDynamics represents the following equation in the continuous time:
+ * \f[
+ * \dot{\tau_{F}} = \ddot{s} ( k_{2} + k_{0} k_{1} (1 - tanh^{2} (k_{1} \dot{s})) )
+ * \f]
+ * since the friction model implemented by this class is defined as:
+ * \f[
+ * \tau_{F} = k_{0} tanh(k_{1} \dot{s}) + k_{2} \dot{s}
+ * \f]
+ * In the discrete time the dynamics is defined as:
+ * \f[
+ * \tau_{F,k+1} = \tau_{F,k} + \Delta T \ddot{s} ( k_{2} + k_{0} k_{1} / cosh^{2}(k_{1} \dot{s,k}) )
+ * \f]
+ */
+class FrictionTorqueStateDynamics : public Dynamics
+{
+ Eigen::VectorXd m_jointVelocityFullModel; /**< Vector of joint velocities. */
+ Eigen::VectorXd m_k0, m_k1, m_k2; /**< Friction parameters (see class description). */
+ double m_dT; /**< Sampling time. */
+ Eigen::VectorXd m_frictionTorqueFullModel; /**< Friction torque vector of full-model. */
+ UKFInput m_ukfInput;
+ 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_coshArgument;
+ Eigen::VectorXd m_coshsquared;
+ Eigen::VectorXd m_k0k1;
+ Eigen::VectorXd m_dotTauF;
+
+public:
+ /*
+ * Constructor
+ */
+ FrictionTorqueStateDynamics();
+
+ /*
+ * Destructor
+ */
+ virtual ~FrictionTorqueStateDynamics();
+
+ /**
+ * 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 |
+ * | `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 |
+ * | `dT` | `double` | Sampling time. | Yes |
+ * @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 state 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;
+};
+
+BLF_REGISTER_UKF_DYNAMICS(FrictionTorqueStateDynamics,
+ ::BipedalLocomotion::Estimators::RobotDynamicsEstimator::Dynamics);
+
+} // namespace RobotDynamicsEstimator
+} // namespace Estimators
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_FRICTION_TORQUE_STATE_DYNAMICS_H

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

@@ -0,0 +1,127 @@
+/**
+ * @file MotorCurrentMeasurementDynamics.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_MOTOR_CURRENT_MEASUREMENT_DYNAMICS_H
+#define BIPEDAL_LOCOMOTION_ESTIMATORS_MOTOR_CURRENT_MEASUREMENT_DYNAMICS_H
+
+#include <memory>
+
+// Eigen
+#include <Eigen/Dense>
+
+#include <BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h>
+
+namespace BipedalLocomotion
+{
+namespace Estimators
+{
+namespace RobotDynamicsEstimator
+{
+
+/**
+ * The MotorCurrentMeasurementDynamics class is a concrete implementation of the Dynamics.
+ * Please use this element to define the measurement of the motor current.
+ * The MotorCurrentMeasurementDynamics represents the following equation in the continuous time:
+ * \f[
+ * \dot{i_{m}} = \tau_{m} / (k_{\tau} r)
+ * \f]
+ */
+class MotorCurrentMeasurementDynamics : public Dynamics
+{
+ UKFInput m_ukfInput;
+ Eigen::VectorXd m_motorTorque; /**< Vector of joint velocities. */
+ Eigen::VectorXd m_currentFrictionTorque; /**< Vector of friction torques. */
+ Eigen::VectorXd m_kTau; /**< Torque constant. */
+ Eigen::VectorXd m_gearRatio; /**< Gearbox reduction ratio. */
+ 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. */
+
+public:
+ /*
+ * Constructor
+ */
+ MotorCurrentMeasurementDynamics();
+
+ /*
+ * Destructor
+ */
+ virtual ~MotorCurrentMeasurementDynamics();
+
+ /**
+ * Initialize the measurement object.
+ * @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 measurement variable contained in the `VariablesHandler` describing 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<const ParametersHandler::IParametersHandler> paramHandler) override;
+
+ /**
+ * Finalize the Dynamics.
+ * @param measurementVariableHandler object describing the variables in the measurement vector.
+ * @note You should call this method after you add ALL the measurement dynamics to the
+ * measurement variable handler.
+ * @return true in case of success, false otherwise.
+ */
+ bool finalize(const System::VariablesHandler& measurementVariableHandler) 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.
+ * @return true if the current state has been updated correctly.
+ */
+ 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;
+};
+
+BLF_REGISTER_UKF_DYNAMICS(MotorCurrentMeasurementDynamics,
+ ::BipedalLocomotion::Estimators::RobotDynamicsEstimator::Dynamics);
+
+} // namespace RobotDynamicsEstimator
+} // namespace Estimators
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_MOTOR_CURRENT_MEASUREMENT_DYNAMICS_H