From 7afd6b120cff1879417e039f2f8ed8a55cb92dc4 Mon Sep 17 00:00:00 2001 From: Ines Date: Wed, 15 Feb 2023 14:05:01 +0100 Subject: [PATCH 01/13] Implement the Dynamics base class, the dynamics classes for the process model, the ukfstate model class. --- ...pedalLocomotionFrameworkDependencies.cmake | 6 +- src/Estimators/CMakeLists.txt | 23 +- .../RobotDynamicsEstimator/Dynamics.h | 198 +++++++++++ .../FrictionTorqueStateDynamics.h | 135 ++++++++ .../JointVelocityStateDynamics.h | 123 +++++++ .../RobotDynamicsEstimator/SubModelDynamics.h | 114 ++++++ .../RobotDynamicsEstimator/UkfState.h | 196 +++++++++++ .../ZeroVelocityDynamics.h | 105 ++++++ src/Estimators/src/Dynamics.cpp | 74 ++++ .../src/FrictionTorqueStateDynamics.cpp | 318 +++++++++++++++++ .../src/JointVelocityStateDynamics.cpp | 264 ++++++++++++++ src/Estimators/src/SubModelDynamics.cpp | 181 ++++++++++ src/Estimators/src/UkfState.cpp | 327 ++++++++++++++++++ src/Estimators/src/ZeroVelocityDynamics.cpp | 169 +++++++++ .../RobotDynamicsEstimator/CMakeLists.txt | 36 ++ .../FrictionTorqueStateDynamicsTest.cpp | 216 ++++++++++++ .../JointVelocityStateDynamicsTest.cpp | 263 ++++++++++++++ .../RobotDynamicsEstimator/UkfStateTest.cpp | 192 ++++++++++ .../ZeroVelocityDynamicsTest.cpp | 101 ++++++ .../RobotDynamicsEstimator/config/config.ini | 1 + .../RobotDynamicsEstimator/config/ukf.ini | 4 + .../config/ukf/ukf_measurement.ini | 46 +++ .../config/ukf/ukf_state.ini | 98 ++++++ 23 files changed, 3175 insertions(+), 15 deletions(-) create mode 100644 src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h create mode 100644 src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h create mode 100644 src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h create mode 100644 src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelDynamics.h create mode 100644 src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h create mode 100644 src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h create mode 100644 src/Estimators/src/Dynamics.cpp create mode 100644 src/Estimators/src/FrictionTorqueStateDynamics.cpp create mode 100644 src/Estimators/src/JointVelocityStateDynamics.cpp create mode 100644 src/Estimators/src/SubModelDynamics.cpp create mode 100644 src/Estimators/src/UkfState.cpp create mode 100644 src/Estimators/src/ZeroVelocityDynamics.cpp create mode 100644 src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp create mode 100644 src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp create mode 100644 src/Estimators/tests/RobotDynamicsEstimator/UkfStateTest.cpp create mode 100644 src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityDynamicsTest.cpp create mode 100644 src/Estimators/tests/RobotDynamicsEstimator/config/ukf.ini create mode 100644 src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini create mode 100644 src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini diff --git a/cmake/BipedalLocomotionFrameworkDependencies.cmake b/cmake/BipedalLocomotionFrameworkDependencies.cmake index a00fa4a60d..0b8fac5646 100644 --- a/cmake/BipedalLocomotionFrameworkDependencies.cmake +++ b/cmake/BipedalLocomotionFrameworkDependencies.cmake @@ -86,6 +86,10 @@ find_package(robometry 1.1.0 QUIET) checkandset_dependency(robometry MINIMUM_VERSION 1.1.0) dependency_classifier(robometry MINIMUM_VERSION 1.1.0 IS_USED ${FRAMEWORK_USE_robometry}) +find_package(BayesFilters QUIET) +checkandset_dependency(BayesFilters) +dependency_classifier(BayesFilters IS_USED ${FRAMEWORK_USE_BayesFilters}) + # required only for some tests find_package(icub-models 1.23.3 QUIET) checkandset_dependency(icub-models) @@ -170,7 +174,7 @@ framework_dependent_option(FRAMEWORK_COMPILE_FloatingBaseEstimators framework_dependent_option(FRAMEWORK_COMPILE_RobotDynamicsEstimator "Compile RobotDynamicsEstimator libraries?" ON - "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_System;FRAMEWORK_COMPILE_ManifConversions;FRAMEWORK_USE_manif" OFF) + "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_System;FRAMEWORK_COMPILE_ManifConversions;FRAMEWORK_USE_manif;FRAMEWORK_USE_BayesFilters" OFF) framework_dependent_option(FRAMEWORK_COMPILE_ManifConversions "Compile manif Conversions libraries?" ON diff --git a/src/Estimators/CMakeLists.txt b/src/Estimators/CMakeLists.txt index ec0785b758..65df49b47e 100644 --- a/src/Estimators/CMakeLists.txt +++ b/src/Estimators/CMakeLists.txt @@ -27,19 +27,14 @@ if(FRAMEWORK_COMPILE_RobotDynamicsEstimator) set(H_PREFIX include/BipedalLocomotion/RobotDynamicsEstimator) add_bipedal_locomotion_library( NAME RobotDynamicsEstimator - SOURCES src/SubModel.cpp - src/SubModelKinDynWrapper.cpp + SOURCES src/SubModel.cpp src/SubModelKinDynWrapper.cpp src/Dynamics.cpp src/ZeroVelocityDynamics.cpp src/FrictionTorqueStateDynamics.cpp + src/JointVelocityStateDynamics.cpp src/UkfState.cpp src/SubModelDynamics.cpp SUBDIRECTORIES tests/RobotDynamicsEstimator - PUBLIC_HEADERS ${H_PREFIX}/SubModel.h - ${H_PREFIX}/SubModelKinDynWrapper.h - PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler - BipedalLocomotion::TextLogging - BipedalLocomotion::ManifConversions - iDynTree::idyntree-high-level - iDynTree::idyntree-core - iDynTree::idyntree-model - iDynTree::idyntree-modelio - Eigen3::Eigen - MANIF::manif -) + PUBLIC_HEADERS ${H_PREFIX}/SubModel.h ${H_PREFIX}/SubModelKinDynWrapper.h ${H_PREFIX}/Dynamics.h ${H_PREFIX}/ZeroVelocityDynamics.h + ${H_PREFIX}/FrictionTorqueStateDynamics.h ${H_PREFIX}/JointVelocityStateDynamics.h + ${H_PREFIX}/UkfState.h ${H_PREFIX}/SubModelDynamics.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 MANIF::manif BayesFilters::BayesFilters + PRIVATE_LINK_LIBRARIES BipedalLocomotion::TextLogging BipedalLocomotion::ManifConversions + ) endif() diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h new file mode 100644 index 0000000000..2aa51777da --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h @@ -0,0 +1,198 @@ +/** + * @file Dynamics.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_DYNAMICS_H +#define BIPEDAL_LOCOMOTION_ESTIMATORS_DYNAMICS_H + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +/** + * BLF_REGISTER_DYNAMICS is a macro that can be used to register a Dynamics. The key of + * the dynamics will be the stringified version of the Dynamics C++ Type + * @param _model the model of the dynamics + * @param _baseModel the base model from which the _model inherits. + */ +#define BLF_REGISTER_DYNAMICS(_model, _baseModel) \ + static std::shared_ptr<_baseModel> _model##FactoryBuilder() \ +{ \ + return std::make_shared<_model>(); \ + }; \ + static std::string _model##BuilderAutoRegHook \ + = ::BipedalLocomotion::Estimators:: \ + RobotDynamicsEstimator::DynamicsFactory::registerBuilder \ + (#_model, _model##FactoryBuilder); + +namespace BipedalLocomotion +{ +namespace Estimators +{ +namespace RobotDynamicsEstimator +{ + +/** + * @brief The UKFInput struct represents the input of the ukf needed to update the dynamics. + */ +struct UKFInput +{ + Eigen::VectorXd robotJointPositions; /**< Vector of joint positions. */ + manif::SE3d robotBasePose; /**< Robot base position and orientation. */ + manif::SE3d::Tangent robotBaseVelocity; /**< Robot base velocity. */ + manif::SE3d::Tangent robotBaseAcceleration; /**< Robot base acceleration. */ +}; + +/** + * UkfInputProvider describes the provider for the inputs of the ukf + */ +class UkfInputProvider : public System::Advanceable +{ +private: + UKFInput m_ukfInput; + +public: + /** + * Get the inputes + * @return A struct containing the inputs for the ukf + */ + const UKFInput& getOutput() const override; + + /** + * Set the state which represents the state of the provider. + * @param input is a struct containing the input of the ukf. + */ + bool setInput(const UKFInput& input) override; + + /** + * @brief Advance the internal state. This may change the value retrievable from getOutput(). + * @return True if the advance is successfull. + */ + bool advance() override; + + /** + * @brief Determines the validity of the object retrieved with getOutput() + * @return True if the object is valid, false otherwise. + */ + bool isOutputValid() const override; + +}; // class UkfProcessInputProvider + +/** + * Dynamics describes a generic dynamic model for a UKF state or measurement. + */ +class Dynamics +{ +protected: + int m_size; /**< Size of the variable associate to the Dynamics object. */ + Eigen::VectorXd m_updatedVariable; /**< Updated variable computed using the dynamic model. */ + std::string m_description{"Generic Dynamics Element"}; /**< String describing the type of the dynamics */ + std::string m_name; /**< Name of dynamics. */ + Eigen::VectorXd m_covariances; /**< Vector of covariances. */ + std::vector m_elements = {}; /**< Elements composing the variable vector. */ + std::string m_dynamicModel; + bool m_isInitialized{false}; /**< True if the dynamics has been initialized. */ + System::VariablesHandler m_stateVariableHandler; /**< Variable handler describing the variables and the sizes in the ukf state/measurement vector. */ + bool m_isStateVariableHandlerSet{false}; /**< True if setVariableHandler is called. */ + UKFInput m_ukfInput; + + /** + * 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. + */ + virtual bool checkStateVariableHandler(); + + +public: + /** + * Initialize the task. + * @param paramHandler pointer to the parameters handler. + * @return True in case of success, false otherwise. + */ + virtual bool + initialize(std::weak_ptr paramHandler); + + /** + * Finalize the Dynamics. + * @param handler object describing the variables in the vector for which all the dynamics are defined. + * @note You should call this method after you add ALL the dynamics to the variable handler. + * @return true in case of success, false otherwise. + */ + virtual bool finalize(const System::VariablesHandler& handler); + + /** + * Set the SubModelKinDynWrapper object. + * @param kinDynWrapper pointer to a SubModelKinDynWrapper object. + * @return True in case of success, false otherwise. + */ + virtual bool setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList); + + /** + * Update the dynamics of the variable. + * @return True in case of success, false otherwise. + */ + virtual bool update(); + + /** + * Get the next value m_updatedVariable. + * @return a const reference to the next variable value. + */ + Eigen::Ref getUpdatedVariable() const; + + /** + * Get the size of the state. + * @return the size of the state. + */ + int size() const; + + /** + * Set the state of the ukf needed to update the dynamics. + * @param ukfState reference to the ukf state. + */ + virtual void setState(const Eigen::Ref ukfState) = 0; + + /** + * Set a `UKFInput` object. + * @param ukfInput reference to the UKFInput struct. + */ + virtual void setInput(const UKFInput & ukfInput) = 0; + + /** + * @brief getCovariance access the covariance `Eigen::VectorXd` associated to the variables described by this dynamics. + * @return the vector of covariances. + */ + Eigen::Ref getCovariance(); + + /** + * Destructor. + */ + virtual ~Dynamics() = default; +}; + +/** + * DynamicsFactory implements the factory design patter for constructing a Dynamics given + * its model. + */ +class DynamicsFactory : public System::Factory +{ + +public: + virtual ~DynamicsFactory() = default; +}; + +} // RobotDynamicsEstimator +} // Estimators +} // BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_DYNAMICS_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h new file mode 100644 index 0000000000..e6629c43d0 --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h @@ -0,0 +1,135 @@ +/** + * @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 +#include +#include + +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 ( k_{2} + k_{0} k_{1} (1 - tanh^{2} (k_{1} \dot{s,k})) ) \ddot{s} + * \f] + */ + +class FrictionTorqueStateDynamics : public Dynamics +{ + Eigen::VectorXd m_jointVelocityFullModel; /**< Vector of joint velocities. */ + Eigen::VectorXd m_currentFrictionTorque; /**< Vector of friction torques. */ + Eigen::VectorXd m_k0, m_k1, m_k2; /**< Friction parameters (see class description). */ + double m_dT; /**< Sampling time. */ + int m_nrOfSubDynamics; /**< Number of sub-dynamics which corresponds to the number of sub-models. */ + std::vector> m_subDynamics; /**< Vector of SubModelInversDynamics objects. */ + Eigen::VectorXd m_motorTorqueFullModel; /**< Motor torque vector of full-model. */ + Eigen::VectorXd m_frictionTorqueFullModel; /**< Friction torque vector of full-model. */ + std::vector m_subModelUpdatedJointAcceleration; /**< Updated joint acceleration of each sub-model. */ + Eigen::VectorXd m_jointAccelerationFullModel; /**< Vector of joint accelerations. */ + bool m_isSubModelListSet{false}; /**< Boolean flag saying if the sub-model list has been set. */ + +protected: + Eigen::VectorXd m_tanhArgument; + Eigen::VectorXd m_tanh; + Eigen::VectorXd m_k0k1; + Eigen::VectorXd m_argParenthesis; + 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 | + * | `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 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 kinDynWrapper pointer to a SubModelKinDynWrapper object. + * @return True in case of success, false otherwise. + */ + bool setSubModels(const std::vector& subModelList, const std::vector>& 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. + * @param ukfState reference to the ukf state. + */ + void setState(const Eigen::Ref ukfState) override; + + /** + * Set a `UKFInput` object. + * @param ukfInput reference to the UKFInput struct. + */ + void setInput(const UKFInput & ukfInput) override; +}; + +BLF_REGISTER_DYNAMICS(FrictionTorqueStateDynamics, ::BipedalLocomotion::Estimators::RobotDynamicsEstimator::Dynamics); + +} // namespace RobotDynamicsEstimator +} // namespace Estimators +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_FRICTION_TORQUE_STATE_DYNAMICS_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h new file mode 100644 index 0000000000..21f8a04f9d --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h @@ -0,0 +1,123 @@ +/** + * @file JointVelocityStateDynamics.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_JOINT_VELOCITY_STATE_DYNAMICS_H +#define BIPEDAL_LOCOMOTION_ESTIMATORS_JOINT_VELOCITY_STATE_DYNAMICS_H + +#include +#include + +#include +#include +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace Estimators +{ +namespace RobotDynamicsEstimator +{ + +/** + * The JointVelocityDynamics class is a concrete implementation of the Dynamics. + * Please use this element if you want to use the robot dynaic model to update the joint dynamics. + * The JointVelocityDynamics represents the following equation in the continuous time: + * \f[ + * \ddot{s} = H^{-1} [\tau_{m} - \tau_{F} + (\sum J^T_{FT} f_{FT}) + + * + (\sum J^T_{ext} f_{ext}) - F^T {}^B \dot v - h ] + * \f] + */ + +class JointVelocityStateDynamics : public Dynamics +{ + int m_nrOfSubDynamics; /**< Number of sub-dynamics which corresponds to the number of sub-models. */ + std::vector> m_subDynamics; /**< Vector of SubModelInversDynamics objects. */ + double m_dT; /**< Sampling time. */ + bool m_isSubModelListSet{false}; /**< Boolean flag saying if the sub-model list has been set. */ + Eigen::VectorXd m_jointVelocityFullModel; /**< Joint velocities of full-model. */ + Eigen::VectorXd m_motorTorqueFullModel; /**< Motor torque vector of full-model. */ + Eigen::VectorXd m_frictionTorqueFullModel; /**< Friction torque vector of full-model. */ + std::vector m_subModelUpdatedJointVelocity; /**< Updated joint velocity of each sub-model. */ + Eigen::VectorXd m_jointAccelerationFullModel; /**< Vector of joint accelerations. */ + std::vector m_subModelUpdatedJointAcceleration; /**< Updated joint acceleration of each sub-model. */ + +public: + /* + * Constructor + */ + JointVelocityStateDynamics(); + + /* + * Destructor + */ + virtual ~JointVelocityStateDynamics(); + + /** + * Set the SubModelKinDynWrapper object. + * @param kinDynWrapper pointer to a SubModelKinDynWrapper object. + * @return True in case of success, false otherwise. + */ + bool setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) override; + + /** + * 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 | + * | `elements` | `vector` | Vector of strings describing the list of sub variables composing the state associated to this dynamics.| No | + * | `dT` | `double` | Sampling time. | Yes | + * @return True in case of success, false otherwise. + */ + bool initialize(std::weak_ptr 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; + + /** + * 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 state. + * @return True in case of success, false otherwise. + */ + bool update() override; + + /** + * Set the state of the ukf needed to update the dynamics. + * @param ukfState reference to the ukf state. + */ + void setState(const Eigen::Ref ukfState) override; + + /** + * Set a `UKFInput` object. + * @param ukfInput reference to the UKFInput struct. + */ + void setInput(const UKFInput & ukfInput) override; + +}; + +BLF_REGISTER_DYNAMICS(JointVelocityStateDynamics, ::BipedalLocomotion::Estimators::RobotDynamicsEstimator::Dynamics); + +} // namespace RobotDynamicsEstimator +} // namespace Estimators +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_JOINT_VELOCITY_STATE_DYNAMICS_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelDynamics.h new file mode 100644 index 0000000000..b29b7e0e2e --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelDynamics.h @@ -0,0 +1,114 @@ +/** + * @file SubModelDynamics.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_SUB_MODEL_DYNAMICS_H +#define BIPEDAL_LOCOMOTION_SUB_MODEL_DYNAMICS_H + +#include +#include +#include + +#include + +#include +#include + +#include +#include + +namespace BipedalLocomotion +{ +namespace Estimators +{ +namespace RobotDynamicsEstimator +{ + +struct SubModelDynamics +{ + std::shared_ptr kinDynWrapper; /**< object containing the kinematics and dynamics properties of a sub-model. */ + SubModel subModel; /**< Sub-model object storing information about the model (joints, sensors, applied wrenches) */ + std::map FTMap; /**< The map contains names of the ft sensors and values of the wrench */ + std::map extContactMap; /**< The map contains names of the ft sensors and values of the wrench */ + manif::SE3d::Tangent baseAcceleration; /**< Velocity of the base of the sub-model. */ + Eigen::VectorXd motorTorque; /**< Motor torque vector of sub-model. */ + Eigen::VectorXd frictionTorque; /**< Friction torque vector of sub-model. */ + Eigen::VectorXd jointVelocity; /**< Joint velocities of sub-model. */ + Eigen::VectorXd totalTorqueFromContacts; /**< Joint torques due to known and unknown contacts on the sub-model. */ + Math::Wrenchd wrench; /**< Joint torques due to a specific contact. */ + Eigen::VectorXd torqueFromContact; /**< Joint torques due to a specific contact. */ + + /* + * Constructor + */ + SubModelDynamics(); + + /* + * Destructor + */ + virtual ~SubModelDynamics(); + + /** + * @brief Set the SubModelKinDynWrapper object. + * @param kinDynWrapper pointer to a SubModelKinDynWrapper object. + * @return true in case of success, false otherwise. + */ + bool setKinDynWrapper(std::shared_ptr kinDynWrapper); + + /** + * @brief Set the SubModel object. + * @param subModel pointer to a SubModel object. + * @return true in case of success, false otherwise. + */ + bool setSubModel(const SubModel& subModel); + + /** + * @brief Initialize the SubDynamics. + * @return true in case of success, false otherwise. + */ + bool initialize(); + + /** + * @brief setState set the state of the variables used to compute the inverse dynamics. + * @param ukfState an Eigen::Vector representing the state computed by the ukf estimator. + * @param jointVelocityFullModel `Eigen::Vector` representing the velocity of the robot joints. + * @param motorTorqueFullModel `Eigen::Vector` representing the torque of the robot motors. + * @param frictionTorqueFullModel `Eigen::Vector` representing the friction torque at the robot motors. + * @param variableHandler handler of the state of the ukf. + */ + void setState(const Eigen::Ref ukfState, + const Eigen::Ref jointVelocityFullModel, + const Eigen::Ref motorTorqueFullModel, + const Eigen::Ref frictionTorqueFullModel, + const System::VariablesHandler& variableHandler); + + /** + * @brief update computes the joint accelerations given by the forward dynamics and the joint velocities given + * by the numerical integration of the joint accelerations. + * @param fullModelBaseAcceleration is a `manif::SE3d::Tangent` representing the acceleration of the base of the full model. + * @param fullModelJointAcceleration is a `Eigen::VectorXd` representing the acceleration of the joints of the full model. + * @param updatedJointAcceleration is a `Eigen::VectorXd` representing the joint accelerations of the sub-model. + * @return true in case of success, false otherwise. + * @note this method computes the joint acceleration of the sub-model, but it requires the joint acceleration of the full model. + * Actually the only joint accelerations needed to compute the ones of this sub-model are the joint accelerations + * of the previous sub-models as they are used to compute the acceleration of the base of this sub-model. The rest of the joint + * accelerations, which are computed here, can be set to zero as those values are not used. + */ + bool update(manif::SE3d::Tangent& fullModelBaseAcceleration, + Eigen::Ref fullModelJointAcceleration, + Eigen::Ref updatedJointAcceleration); + + /** + * @brief Compute the contribution of external contacts on the joint torques. + */ + void computeTotalTorqueFromContacts(); +}; + +} // RobotDynamicsEstimator +} // Estimators +} // BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_SUB_MODEL_DYNAMICS_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h new file mode 100644 index 0000000000..435490b0d2 --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h @@ -0,0 +1,196 @@ +/** + * @file UkfState.h + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_UKF_STATE_H +#define BIPEDAL_LOCOMOTION_ESTIMATORS_UKF_STATE_H + +#include +#include +#include + +// BayesFilters +#include + +// BLF +#include +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace Estimators +{ +namespace RobotDynamicsEstimator +{ + +/** + * UkfState is a concrete class that represents the State of the estimator. + * The user should build the dynamic model of the state setting a variable handler + * describing the variables composing the state, the list of the dynamic model associated + * to each variable in the variable handler, and the matrix of covariances + * associated to the state. The user should set also a ukf input provider + * which provides the inputs needed to update the dynamics. + */ +class UkfState : public bfl::AdditiveStateModel +{ + /** + * Private implementation + */ + struct Impl; + std::unique_ptr m_pimpl; + +public: + /** + * Constructor. + */ + UkfState(); + + /** + * Destructor. + */ + virtual ~UkfState(); + + /** + * Build the ukf state model + * @param kinDyn a pointer to an iDynTree::KinDynComputations object that will be shared among + * all the dynamics. + * @param subModelList a vector of pairs (SubModel, SubModelKinDynWrapper) that will be shared among all the dynamics. + * @param handler pointer to the IParametersHandler interface. + * @note the following parameters are required by the class + * | Parameter Name | Type | Description | Mandatory | + * |:------------------------------:|:---------------:|:----------------------------------------------------------------------------------------------:|:---------:| + * | `dT` | `double` | Sampling time. | Yes | + * | `dynamics_list` |`vector` | List of dynamics composing the state model. | Yes | + * For **each** dynamics listed in the parameter `dynamics_list` the user must specified all the parameters + * required by the dynamics itself but `dT` since is already specified in the parent group. + * Moreover the following parameters are required for each dynamics. + * | Group | Parameter Name | Type | Description | Mandatory | + * |:-------------:|:------------------------------:|:--------------------:|:----------------------------------------------------------------------------------------------:|:---------:| + * |`DYNAMICS_NAME`| `name` | `string` | String representing the name of the dynamics. | Yes | + * |`DYNAMICS_NAME`| `elements` |`std::vector` | Vector of strings representing the elements composing the specific dynamics. | No | + * |`DYNAMICS_NAME`| `covariance` |`std::vector` | Vector of double containing the covariance associated to each element. | Yes | + * |`DYNAMICS_NAME`| `dynamic_model` | `string` | String representing the type of dynamics. The string should match the name of the C++ class. | Yes | + * |`DYNAMICS_NAME`| `friction_k0` |`std::vector` | Vector of double containing the coefficient k0 of the friction model of each element. | No | + * |`DYNAMICS_NAME`| `friction_k1` |`std::vector` | Vector of double containing the coefficient k1 of the friction model of each element. | No | + * |`DYNAMICS_NAME`| `friction_k2` |`std::vector` | Vector of double containing the coefficient k2 of the friction model of each element. | No | + * `DYNAMICS_NAME` is a placeholder for the name of the dynamics contained in the `dynamics_list` list. + * `name` can contain only the following values ("ds", "tau_m", "tau_F", "*_ft_sensor", "*_ft_sensor_bias", "*_ft_acc_bias", "*_ftgyro_bias"). + * @note The following `ini` file presents an example of the configuration that can be used to + * build the UkfState. + * + * UkfState.ini + * + * dynamics_list ("JOINT_VELOCITY", "FRICTION_TORQUE", "RIGHT_LEG_FT", "RIGHT_FOOT_REAR_GYRO_BIAS") + * + * [JOINT_VELOCITY] + * name "ds" + * elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") + * covariance (1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3) + * dynamic_model "JointVelocityDynamics" + * + * [FRICTION_TORQUE] + * name "tau_F" + * elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") + * covariance (1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-1) + * dynamic_model "FrictionTorqueDynamics" + * friction_k0 (9.106, 5.03, 4.93, 12.88, 14.34, 1.12) + * friction_k1 (200.0, 6.9, 200.0, 59.87, 200.0, 200.0) + * friction_k2 (1.767, 5.64, 0.27, 2.0, 3.0, 0.0) + * + * [RIGHT_LEG_FT] + * name "r_leg_ft_sensor" + * elements ("fx", "fy", "fz", "mx", "my", "mz") + * covariance (1e-3, 1e-3, 1e-3, 1e-4, 1e-4, 1e-4) + * dynamic_model "ZeroVelocityDynamics" + * + * [RIGHT_FOOT_REAR_GYRO_BIAS] + * name "r_foot_rear_ft_gyro_bias" + * elements ("x", "y", "z") + * covariance (8.2e-8, 1e-2, 9.3e-3) + * dynamic_model "ZeroVelocityDynamics" + * + * ~~~~~ + * @return a std::unique_ptr to the UkfState. + * In case of issues, an empty BipedalLocomotion::System::VariablesHandler + * and an invalid pointer will be returned. + */ + static std::unique_ptr build(std::weak_ptr handler, + std::shared_ptr kinDynFullModel, + const std::vector& subModelList, + const std::vector>& kinDynWrapperList); + + /** + * Initialize the ukf state model. + * @param handler pointer to the IParametersHandler interface. + * @note the following parameters are required by the class + * | Parameter Name | Type | Description | Mandatory | + * |:------------------------------:|:---------------:|:----------------------------------------------------------------------------------------------:|:---------:| + * | `dT` | `double` | Sampling time. | Yes | + * @return True in case of success, false otherwise. + */ + bool initialize(std::weak_ptr handler); + + /** + * Finalize the UkfState. + * @param handler variable handler. + * @note You should call this method after you initialize the UkfState. + * @return true in case of success, false otherwise. + */ + bool finalize(const System::VariablesHandler& handler); + + /** + * @brief setUkfInputProvider set the provider for the ukf input. + * @param ukfInputProvider a pointer to a structure containing the joint positions and the robot base pose, velocity and acceleration. + */ + void setUkfInputProvider(std::shared_ptr ukfInputProvider); + + /** + * @brief getStateVariableHandler access the `System::VariablesHandler` instance created during the initialization phase. + * @return the state variable handler containing all the state variables and their sizes and offsets. + */ + System::VariablesHandler getStateVariableHandler(); + + /** + * @brief propagate implements the predict of the ukf + * @param cur_states is the state computed at the previous step + * @param prop_states is the predicted state + */ + void propagate(const Eigen::Ref& cur_states, Eigen::Ref prop_states) override; + + /** + * @brief getNoiseCovarianceMatrix access the `Eigen::MatrixXd` representing the process covariance matrix. + * @return the process noise covariance matrix. + */ + Eigen::MatrixXd getNoiseCovarianceMatrix() override; + + /** + * @brief setProperty is not implemented. + * @param property is a string. + * @return false as it is not implemented. + */ + bool setProperty(const std::string& property) override { return false; }; + + /** + * @brief getStateDescription access the `bfl::VectorDescription`. + * @return the state vector description. + */ + bfl::VectorDescription getStateDescription() override; + + /** + * @brief getStateSize access the length of the state vector + * @return the length of state vector + */ + std::size_t getStateSize(); + +}; // class UKFModel + +} // namespace RobotDynamicsEstimator +} // namespace Estimators +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_UKF_STATE_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h new file mode 100644 index 0000000000..553a2ff0cd --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h @@ -0,0 +1,105 @@ +/** + * @file ZeroVelocityDynamics.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_ZERO_VELOCITY_DYNAMICS_H +#define BIPEDAL_LOCOMOTION_ESTIMATORS_ZERO_VELOCITY_DYNAMICS_H + +#include +#include + +namespace BipedalLocomotion +{ +namespace Estimators +{ +namespace RobotDynamicsEstimator +{ + +/** + * The ZeroVelocityDynamics class is a concrete implementation of the Dynamics. + * Please use this element if you do not know the specific dynamics of a state variable. + * The ZeroVelocityDynamics represents the following equation in the continuous time: + * \f[ + * \dot{x} = 0 + * \f] + * In the discrete time the following dynamics assigns the current state to the next state: + * \f[ + * x_{k+1} = x_{k} + * \f] + */ + +class ZeroVelocityDynamics : public Dynamics +{ +protected: + Eigen::VectorXd m_currentState; /**< Current state. */ + 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. */ + + /** + * 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; + +public: + /** + * 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 | + * | `elements` | `vector` | Vector of strings describing the list of sub variables composing the state associated to this dynamics.| No | + * | `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; + + /** + * 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 kinDynWrapper pointer to a SubModelKinDynWrapper object. + * @return True in case of success, false otherwise. + */ + bool setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) 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. + * @param ukfState reference to the ukf state. + */ + void setState(const Eigen::Ref ukfState) override; + + /** + * Set a `UKFInput` object. + * @param ukfInput reference to the UKFInput struct. + */ + void setInput(const UKFInput & ukfInput) override; + +}; // class ZeroVelocityDynamics + +BLF_REGISTER_DYNAMICS(ZeroVelocityDynamics, ::BipedalLocomotion::Estimators::RobotDynamicsEstimator::Dynamics); + +} // namespace RobotDynamicsEstimator +} // namespace Estimators +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_ZERO_VELOCITY_DYNAMICS_H diff --git a/src/Estimators/src/Dynamics.cpp b/src/Estimators/src/Dynamics.cpp new file mode 100644 index 0000000000..d751532a4e --- /dev/null +++ b/src/Estimators/src/Dynamics.cpp @@ -0,0 +1,74 @@ +/** + * @file Dynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::ParametersHandler; + +const RDE::UKFInput& RDE::UkfInputProvider::getOutput() const +{ + return m_ukfInput; +} + +bool RDE::UkfInputProvider::advance() +{ + return true; +} + +bool RDE::UkfInputProvider::setInput(const UKFInput& input) +{ + m_ukfInput = input; + + return true; +} + +bool RDE::UkfInputProvider::isOutputValid() const +{ + return m_ukfInput.robotJointPositions.size() != 0; +} + +bool RDE::Dynamics::initialize(std::weak_ptr paramHandler) +{ + return true; +} + +bool RDE::Dynamics::finalize(const System::VariablesHandler& stateVariableHandler) +{ + return true; +} + +bool RDE::Dynamics::setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) +{ + return true; +} + +bool RDE::Dynamics::update() +{ + return true; +} + +Eigen::Ref RDE::Dynamics::getUpdatedVariable() const +{ + return m_updatedVariable; +} + +int RDE::Dynamics::size() const +{ + return m_size; +} + +Eigen::Ref RDE::Dynamics::getCovariance() +{ + return m_covariances; +} + +bool RDE::Dynamics::checkStateVariableHandler() +{ + return true; +} diff --git a/src/Estimators/src/FrictionTorqueStateDynamics.cpp b/src/Estimators/src/FrictionTorqueStateDynamics.cpp new file mode 100644 index 0000000000..6cd32a3ab6 --- /dev/null +++ b/src/Estimators/src/FrictionTorqueStateDynamics.cpp @@ -0,0 +1,318 @@ +/** + * @file FrictionTorqueStateDynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include + +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +RDE::FrictionTorqueStateDynamics::FrictionTorqueStateDynamics() = default; + +RDE::FrictionTorqueStateDynamics::~FrictionTorqueStateDynamics() = default; + +bool RDE::FrictionTorqueStateDynamics::initialize(std::weak_ptr paramHandler) +{ + constexpr auto errorPrefix = "[FrictionTorqueStateDynamics::initialize]"; + + auto ptr = paramHandler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is not valid.", errorPrefix); + return false; + } + + // Set the state dynamics name + if (!ptr->getParameter("name", m_name)) + { + log()->error("{} Error while retrieving the name variable.", errorPrefix); + return false; + } + + // Set the state process covariance + if (!ptr->getParameter("covariance", m_covariances)) + { + log()->error("{} Error while retrieving the covariance variable.", errorPrefix); + return false; + } + + // Set the dynamic model type + if (!ptr->getParameter("dynamic_model", m_dynamicModel)) + { + log()->error("{} Error while retrieving the dynamic_model variable.", errorPrefix); + return false; + } + + // Set the list of elements if it exists + if (!ptr->getParameter("elements", m_elements)) + { + log()->info("{} Variable elements not found.", errorPrefix); + m_elements = {}; + } + + // Set the friction parameters + if (!ptr->getParameter("friction_k0", m_k0)) + { + log()->error("{} Error while retrieving the friction_k0 variable.", errorPrefix); + return false; + } + + if (!ptr->getParameter("friction_k1", m_k1)) + { + log()->error("{} Error while retrieving the friction_k1 variable.", errorPrefix); + return false; + } + + if (!ptr->getParameter("friction_k2", m_k2)) + { + log()->error("{} Error while retrieving the friction_k2 variable.", errorPrefix); + return false; + } + + if (!ptr->getParameter("sampling_time", m_dT)) + { + log()->error("{} Error while retrieving the sampling_time variable.", errorPrefix); + return false; + } + + m_description = "Friction torque state dynamics"; + + m_isInitialized = true; + + return true; +} + +bool RDE::FrictionTorqueStateDynamics::finalize(const System::VariablesHandler &stateVariableHandler) +{ + constexpr auto errorPrefix = "[FrictionTorqueStateDynamics::finalize]"; + + if (!m_isInitialized) + { + log()->error("{} Please initialize the dynamics before calling finalize.", errorPrefix); + return false; + } + + if (stateVariableHandler.getNumberOfVariables() == 0) + { + log()->error("{} The state variable handler is empty.", errorPrefix); + return false; + } + + m_stateVariableHandler = stateVariableHandler; + + if (!checkStateVariableHandler()) + { + log()->error("{} The state variable handler is not valid.", errorPrefix); + return false; + } + + m_size = m_covariances.size(); + + m_motorTorqueFullModel.resize(m_stateVariableHandler.getVariable("tau_m").size); + m_motorTorqueFullModel.setZero(); + + m_frictionTorqueFullModel.resize(m_stateVariableHandler.getVariable("tau_F").size); + m_frictionTorqueFullModel.setZero(); + + m_jointAccelerationFullModel.resize(m_stateVariableHandler.getVariable("ds").size); + m_jointAccelerationFullModel.setZero(); + + m_jointVelocityFullModel.resize(m_stateVariableHandler.getVariable("ds").size); + m_jointVelocityFullModel.setZero(); + + m_subModelUpdatedJointAcceleration.resize(m_nrOfSubDynamics); + + for (int idx = 0; idx < m_nrOfSubDynamics; idx++) + { + m_subModelUpdatedJointAcceleration[idx].resize(m_subDynamics[idx]->subModel.getJointMapping().size()); + m_subModelUpdatedJointAcceleration[idx].setZero(); + } + + m_currentFrictionTorque.resize(m_size); + m_currentFrictionTorque.setZero(); + + m_updatedVariable.resize(m_size); + m_updatedVariable.setZero(); + + m_tanhArgument.resize(m_size); + m_tanhArgument.setZero(); + + m_tanh.resize(m_size); + m_tanh.setZero(); + + m_k0k1.resize(m_size); + m_k0k1.setZero(); + + m_argParenthesis.resize(m_size); + m_argParenthesis.setZero(); + + m_dotTauF.resize(m_size); + m_dotTauF.setZero(); + + return true; +} + +bool RDE::FrictionTorqueStateDynamics::setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) +{ + constexpr auto errorPrefix = "[FrictionTorqueStateDynamics::setSubModels]"; + + for (int subModelIdx = 0; subModelIdx < subModelList.size(); subModelIdx++) + { + if (subModelList[subModelIdx].getModel().getNrOfDOFs() > 0) + { + m_subDynamics.emplace_back(std::make_unique()); + + if(!m_subDynamics[subModelIdx]->setSubModel(subModelList[subModelIdx])) + { + log()->error("{} The submodel at index {} is not valid.", errorPrefix, subModelIdx); + return false; + } + + if(!m_subDynamics[subModelIdx]->setKinDynWrapper(kinDynWrapperList[subModelIdx])) + { + log()->error("{} The submodel kindyn wrapper at index {} is not valid.", errorPrefix, subModelIdx); + return false; + } + } + } + + m_nrOfSubDynamics = m_subDynamics.size(); + + m_isSubModelListSet = true; + + for (int subModelIdx = 0; subModelIdx < m_nrOfSubDynamics; subModelIdx++) + { + if (!m_subDynamics.at(subModelIdx)->initialize()) + { + log()->error("{} Cannot initialize the joint velocity dynamics of the sub-model {}.", errorPrefix, subModelIdx); + return false; + } + } + + return true; +} + +bool RDE::FrictionTorqueStateDynamics::checkStateVariableHandler() +{ + constexpr auto errorPrefix = "[FrictionTorqueStateDynamics::checkStateVariableHandler]"; + + if (!m_isSubModelListSet) + { + log()->error("{} Set the sub-model list before setting the variable handler", errorPrefix); + return false; + } + + if (!m_stateVariableHandler.getVariable("tau_m").isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `tau_m`.", errorPrefix); + return false; + } + + // Check if the variable handler contains the variables used by this dynamics + if (!m_stateVariableHandler.getVariable("tau_F").isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `tau_F`.", errorPrefix); + return false; + } + + if (!m_stateVariableHandler.getVariable("ds").isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `ds`.", errorPrefix); + return false; + } + + // check if all the sensors are part of the sub-model + for (int subModelIdx = 0; subModelIdx < m_nrOfSubDynamics; subModelIdx++) + { + for (int ftIdx = 0; ftIdx < m_subDynamics.at(subModelIdx)->subModel.getNrOfFTSensor(); ftIdx++) + { + if (!m_stateVariableHandler.getVariable(m_subDynamics.at(subModelIdx)->subModel.getFTSensor(ftIdx).name).isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_subDynamics.at(subModelIdx)->subModel.getFTSensor(ftIdx).name); + return false; + } + } + + for (int contactIdx = 0; contactIdx < m_subDynamics.at(subModelIdx)->subModel.getNrOfExternalContact(); contactIdx++) + { + if (!m_stateVariableHandler.getVariable(m_subDynamics.at(subModelIdx)->subModel.getExternalContact(contactIdx)).isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_subDynamics.at(subModelIdx)->subModel.getExternalContact(contactIdx)); + return false; + } + } + } + + return true; +} + +bool RDE::FrictionTorqueStateDynamics::update() +{ + constexpr auto errorPrefix = "[FrictionTorqueStateDynamics::update]"; + + // compute joint acceleration per each sub-model + for (int subDynamicsIdx = 0; subDynamicsIdx < m_subDynamics.size(); subDynamicsIdx++) + { + if (!m_subDynamics[subDynamicsIdx]->update(m_ukfInput.robotBaseAcceleration, m_jointAccelerationFullModel, m_subModelUpdatedJointAcceleration[subDynamicsIdx])) + { + log()->error("{} Error updating the joint velocity dynamics for the sub-model {}.", errorPrefix, subDynamicsIdx); + return false; + } + + for (int jointIdx = 0; jointIdx < m_subDynamics[subDynamicsIdx]->subModel.getJointMapping().size(); jointIdx++) + { + m_jointAccelerationFullModel[m_subDynamics[subDynamicsIdx]->subModel.getJointMapping()[jointIdx]] = m_subModelUpdatedJointAcceleration[subDynamicsIdx][jointIdx]; + } + } + + // k_{1} \dot{s,k} + m_tanhArgument = m_k1.array() * m_jointVelocityFullModel.array(); + + // tanh (k_{1} \dot{s,k})) + m_tanh = m_tanhArgument.array().tanh(); + + // 1 - tanh^{2} (k_{1} \dot{s,k}) + m_argParenthesis = 1 - m_tanh.array().square(); + + // k_{0} k_{1} + m_k0k1 = m_k0.array() * m_k1.array(); + + // \ddot{s,k} ( k_{2} + k_{0} k_{1} (1 - tanh^{2} (k_{1} \dot{s,k})) ) + m_dotTauF = m_jointAccelerationFullModel.array() * ( m_k2.array() + m_k0k1.array() * m_argParenthesis.array() ); + + // \tau_{F,k+1} = \tau_{F,k} + \Delta T * \dot{\tau_{F,k}} + m_updatedVariable = m_currentFrictionTorque + m_dT * m_dotTauF; + + return true; +} + +void RDE::FrictionTorqueStateDynamics::setState(const Eigen::Ref ukfState) +{ + m_jointVelocityFullModel = ukfState.segment(m_stateVariableHandler.getVariable("ds").offset, + m_stateVariableHandler.getVariable("ds").size); + + m_motorTorqueFullModel = ukfState.segment(m_stateVariableHandler.getVariable("tau_m").offset, + m_stateVariableHandler.getVariable("tau_m").size); + + m_frictionTorqueFullModel = ukfState.segment(m_stateVariableHandler.getVariable("tau_F").offset, + m_stateVariableHandler.getVariable("tau_F").size); + + for (int subDynamicsIdx = 0; subDynamicsIdx < m_subDynamics.size(); subDynamicsIdx++) + { + m_subDynamics.at(subDynamicsIdx)->setState(ukfState, + m_jointVelocityFullModel, + m_motorTorqueFullModel, + m_frictionTorqueFullModel, + m_stateVariableHandler); + } +} + +void RDE::FrictionTorqueStateDynamics::setInput(const UKFInput& ukfInput) +{ + m_ukfInput = ukfInput; +} diff --git a/src/Estimators/src/JointVelocityStateDynamics.cpp b/src/Estimators/src/JointVelocityStateDynamics.cpp new file mode 100644 index 0000000000..697c9a1681 --- /dev/null +++ b/src/Estimators/src/JointVelocityStateDynamics.cpp @@ -0,0 +1,264 @@ +/** + * @file JointVelocityStateDynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include + +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +RDE::JointVelocityStateDynamics::JointVelocityStateDynamics() = default; + +RDE::JointVelocityStateDynamics::~JointVelocityStateDynamics() = default; + +bool RDE::JointVelocityStateDynamics::setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) +{ + constexpr auto errorPrefix = "[JointVelocityStateDynamics::setSubModels]"; + + for (int subModelIdx = 0; subModelIdx < subModelList.size(); subModelIdx++) + { + if (subModelList[subModelIdx].getModel().getNrOfDOFs() > 0) + { + m_subDynamics.emplace_back(std::make_unique()); + + if(!m_subDynamics[subModelIdx]->setSubModel(subModelList[subModelIdx])) + { + log()->error("{} The submodel at index {} is not valid.", errorPrefix, subModelIdx); + return false; + } + + if(!m_subDynamics[subModelIdx]->setKinDynWrapper(kinDynWrapperList[subModelIdx])) + { + log()->error("{} The submodel kindyn wrapper at index {} is not valid.", errorPrefix, subModelIdx); + return false; + } + } + } + + m_nrOfSubDynamics = m_subDynamics.size(); + + m_isSubModelListSet = true; + + for (int subModelIdx = 0; subModelIdx < m_nrOfSubDynamics; subModelIdx++) + { + if (!m_subDynamics.at(subModelIdx)->initialize()) + { + log()->error("{} Cannot initialize the joint velocity dynamics of the sub-model {}.", errorPrefix, subModelIdx); + return false; + } + } + + return true; +} + +bool RDE::JointVelocityStateDynamics::checkStateVariableHandler() +{ + constexpr auto errorPrefix = "[JointVelocityStateDynamics::checkStateVariableHandler]"; + + if (!m_isSubModelListSet) + { + log()->error("{} Set the sub-model list before setting the variable handler", errorPrefix); + return false; + } + + // Check if the variable handler contains the variables used by this dynamics + if (!m_stateVariableHandler.getVariable("tau_m").isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `tau_m`.", errorPrefix); + return false; + } + + if (!m_stateVariableHandler.getVariable("tau_F").isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `tau_F`.", errorPrefix); + return false; + } + + for (int subModelIdx = 0; subModelIdx < m_nrOfSubDynamics; subModelIdx++) + { + for (int ftIdx = 0; ftIdx < m_subDynamics.at(subModelIdx)->subModel.getNrOfFTSensor(); ftIdx++) + { + if (!m_stateVariableHandler.getVariable(m_subDynamics.at(subModelIdx)->subModel.getFTSensor(ftIdx).name).isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_subDynamics.at(subModelIdx)->subModel.getFTSensor(ftIdx).name); + return false; + } + } + + for (int contactIdx = 0; contactIdx < m_subDynamics.at(subModelIdx)->subModel.getNrOfExternalContact(); contactIdx++) + { + if (!m_stateVariableHandler.getVariable(m_subDynamics.at(subModelIdx)->subModel.getExternalContact(contactIdx)).isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_subDynamics.at(subModelIdx)->subModel.getExternalContact(contactIdx)); + return false; + } + } + } + + return true; +} + +bool RDE::JointVelocityStateDynamics::initialize(std::weak_ptr paramHandler) +{ + constexpr auto errorPrefix = "[JointVelocityStateDynamics::initialize]"; + + auto ptr = paramHandler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is not valid.", errorPrefix); + return false; + } + + // Set the state dynamics name + if (!ptr->getParameter("name", m_name)) + { + log()->error("{} Error while retrieving the name variable.", errorPrefix); + return false; + } + + // Set the state process covariance + if (!ptr->getParameter("covariance", m_covariances)) + { + log()->error("{} Error while retrieving the covariance variable.", errorPrefix); + return false; + } + + // Set the dynamic model type + if (!ptr->getParameter("dynamic_model", m_dynamicModel)) + { + log()->error("{} Error while retrieving the dynamic_model variable.", errorPrefix); + return false; + } + + // Set the list of elements if it exists + if (!ptr->getParameter("elements", m_elements)) + { + log()->info("{} Variable elements not found.", errorPrefix); + m_elements = {}; + } + + if (!ptr->getParameter("sampling_time", m_dT)) + { + log()->info("{} Error while retrieving the sampling_time variable.", errorPrefix); + m_elements = {}; + } + + m_description = "Joint velocity state dynamics depending on the robot dynamic model"; + + m_isInitialized = true; + + return true; +} + +bool RDE::JointVelocityStateDynamics::finalize(const System::VariablesHandler &stateVariableHandler) +{ + constexpr auto errorPrefix = "[JointVelocityStateDynamics::finalize]"; + + if (!m_isInitialized) + { + log()->error("{} Please initialize the dynamics before calling finalize.", errorPrefix); + return false; + } + + if (stateVariableHandler.getNumberOfVariables() == 0) + { + log()->error("{} The state variable handler is empty.", errorPrefix); + return false; + } + + m_stateVariableHandler = stateVariableHandler; + + if (!checkStateVariableHandler()) + { + log()->error("{} The state variable handler is not valid.", errorPrefix); + return false; + } + + m_size = m_covariances.size(); + + m_motorTorqueFullModel.resize(m_stateVariableHandler.getVariable("tau_m").size); + m_motorTorqueFullModel.setZero(); + + m_frictionTorqueFullModel.resize(m_stateVariableHandler.getVariable("tau_F").size); + m_frictionTorqueFullModel.setZero(); + + m_jointVelocityFullModel.resize(m_stateVariableHandler.getVariable("ds").size); + m_jointVelocityFullModel.setZero(); + + m_jointAccelerationFullModel.resize(m_stateVariableHandler.getVariable("ds").size); + m_jointAccelerationFullModel.setZero(); + + m_updatedVariable.resize(m_stateVariableHandler.getVariable("ds").size); + m_updatedVariable.setZero(); + + m_subModelUpdatedJointVelocity.resize(m_nrOfSubDynamics); + m_subModelUpdatedJointAcceleration.resize(m_nrOfSubDynamics); + + for (int idx = 0; idx < m_nrOfSubDynamics; idx++) + { + m_subModelUpdatedJointVelocity[idx].resize(m_subDynamics[idx]->subModel.getJointMapping().size()); + m_subModelUpdatedJointVelocity[idx].setZero(); + + m_subModelUpdatedJointAcceleration[idx].resize(m_subDynamics[idx]->subModel.getJointMapping().size()); + m_subModelUpdatedJointAcceleration[idx].setZero(); + } + + return true; +} + + +bool RDE::JointVelocityStateDynamics::update() +{ + constexpr auto errorPrefix = "[JointVelocityStateDynamics::update]"; + + // compute joint acceleration per each sub-model + for (int subDynamicsIdx = 0; subDynamicsIdx < m_subDynamics.size(); subDynamicsIdx++) + { + if (!m_subDynamics[subDynamicsIdx]->update(m_ukfInput.robotBaseAcceleration, m_jointAccelerationFullModel, m_subModelUpdatedJointAcceleration[subDynamicsIdx])) + { + log()->error("{} Error updating the joint velocity dynamics for the sub-model {}.", errorPrefix, subDynamicsIdx); + return false; + } + + m_subModelUpdatedJointVelocity[subDynamicsIdx] = m_subModelUpdatedJointVelocity[subDynamicsIdx] + m_dT * m_subModelUpdatedJointAcceleration[subDynamicsIdx]; + + for (int jointIdx = 0; jointIdx < m_subDynamics[subDynamicsIdx]->subModel.getJointMapping().size(); jointIdx++) + { + m_jointAccelerationFullModel[m_subDynamics[subDynamicsIdx]->subModel.getJointMapping()[jointIdx]] = m_subModelUpdatedJointAcceleration[subDynamicsIdx][jointIdx]; + + m_updatedVariable[m_subDynamics[subDynamicsIdx]->subModel.getJointMapping()[jointIdx]] = m_subModelUpdatedJointVelocity[subDynamicsIdx][jointIdx]; + } + } + + 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_motorTorqueFullModel = ukfState.segment(m_stateVariableHandler.getVariable("tau_m").offset, + m_stateVariableHandler.getVariable("tau_m").size); + + m_frictionTorqueFullModel = ukfState.segment(m_stateVariableHandler.getVariable("tau_F").offset, + m_stateVariableHandler.getVariable("tau_F").size); + + for (int subDynamicsIdx = 0; subDynamicsIdx < m_subDynamics.size(); subDynamicsIdx++) + { + m_subDynamics.at(subDynamicsIdx)->setState(ukfState, + m_jointVelocityFullModel, + m_motorTorqueFullModel, + m_frictionTorqueFullModel, + m_stateVariableHandler); + } +} + +void RDE::JointVelocityStateDynamics::setInput(const UKFInput& ukfInput) +{ + m_ukfInput = ukfInput; +} diff --git a/src/Estimators/src/SubModelDynamics.cpp b/src/Estimators/src/SubModelDynamics.cpp new file mode 100644 index 0000000000..d25deffea3 --- /dev/null +++ b/src/Estimators/src/SubModelDynamics.cpp @@ -0,0 +1,181 @@ +/** + * @file SubModelDynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include + +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +RDE::SubModelDynamics::SubModelDynamics() = default; + +RDE::SubModelDynamics::~SubModelDynamics() = default; + +/** + * Set the SubModelKinDynWrapper object. + * Parameter kinDynWrapper pointer to a SubModelKinDynWrapper object. + * Return true in case of success, false otherwise. + */ +bool RDE::SubModelDynamics::setKinDynWrapper(std::shared_ptr kinDynWrapper) +{ + constexpr auto errorPrefix = "[SubModelDynamics::setKinDynWrapper]"; + + this->kinDynWrapper = kinDynWrapper; + + if (kinDynWrapper == nullptr) + { + log()->error("{} Error while setting the `SubModelKinDynWrapper` object.", errorPrefix); + return false; + } + + return true; +} + +bool RDE::SubModelDynamics::setSubModel(const RDE::SubModel& subModel) +{ + constexpr auto errorPrefix = "[SubModelDynamics::setSubModel]"; + + this->subModel = subModel; + + if (!subModel.isValid()) + { + log()->error("{} Set a valid `SubModel` object.", errorPrefix); + return false; + } + + return true; +} + + +bool RDE::SubModelDynamics::initialize() +{ + constexpr auto errorPrefix = "[SubModelDynamics::initialize]"; + + // Inialize map of the ft sensors + if (!subModel.isValid()) + { + log()->error("{} Set a valid `SubModel` object before calling initialize.", errorPrefix); + return false; + } + + for (int idx = 0; idx < subModel.getNrOfFTSensor(); idx++) + { + FTMap[subModel.getFTSensor(idx).name].setZero(); + } + + // Inialize map of the external contacts + for (int idx = 0; idx < subModel.getNrOfExternalContact(); idx++) + { + extContactMap[subModel.getExternalContact(idx)].setZero(); + } + + // Resize and initialize variables + baseAcceleration.setZero(); + + motorTorque.resize(subModel.getModel().getNrOfDOFs()); + motorTorque.setZero(); + + frictionTorque.resize(subModel.getModel().getNrOfDOFs()); + frictionTorque.setZero(); + + jointVelocity.resize(subModel.getModel().getNrOfDOFs()); + jointVelocity.setZero(); + + totalTorqueFromContacts.resize(subModel.getModel().getNrOfDOFs()); + totalTorqueFromContacts.setZero(); + + torqueFromContact.resize(subModel.getModel().getNrOfDOFs()); + torqueFromContact.setZero(); + + wrench.setZero(); + + return true; +} + +void RDE::SubModelDynamics::setState(const Eigen::Ref ukfState, + const Eigen::Ref jointVelocityFullModel, + const Eigen::Ref motorTorqueFullModel, + const Eigen::Ref frictionTorqueFullModel, + const System::VariablesHandler& variableHandler) +{ + for (int idx = 0; idx < subModel.getModel().getNrOfDOFs(); idx++) + { + jointVelocity(idx) = jointVelocityFullModel(subModel.getJointMapping().at(idx)); + } + + for (int idx = 0; idx < subModel.getModel().getNrOfDOFs(); idx++) + { + motorTorque(idx) = motorTorqueFullModel(subModel.getJointMapping().at(idx)); + } + + for (int idx = 0; idx < subModel.getModel().getNrOfDOFs(); idx++) + { + frictionTorque(idx) = frictionTorqueFullModel(subModel.getJointMapping().at(idx)); + } + + for (int idx = 0; idx < subModel.getNrOfFTSensor(); idx++) + { + FTMap[subModel.getFTSensor(idx).name] = ukfState.segment(variableHandler.getVariable(subModel.getFTSensor(idx).name).offset, + variableHandler.getVariable(subModel.getFTSensor(idx).name).size); + } + + for (int idx = 0; idx < subModel.getNrOfExternalContact(); idx++) + { + FTMap[subModel.getExternalContact(idx)] = ukfState.segment(variableHandler.getVariable(subModel.getExternalContact(idx)).offset, + variableHandler.getVariable(subModel.getExternalContact(idx)).size); + } +} + +bool RDE::SubModelDynamics::update(manif::SE3d::Tangent& fullModelBaseAcceleration, + Eigen::Ref fullModelJointAcceleration, + Eigen::Ref updatedJointAcceleration) +{ + constexpr auto errorPrefix = "[SubModelDynamics::update]"; + + computeTotalTorqueFromContacts(); + + if (!kinDynWrapper->getBaseAcceleration(fullModelBaseAcceleration, fullModelJointAcceleration, baseAcceleration)) + { + log()->error("{} Cannot compute the sub-model base acceleration.", errorPrefix); + return false; + } + + if (!kinDynWrapper->forwardDynamics(motorTorque, + frictionTorque, + totalTorqueFromContacts, + baseAcceleration.coeffs(), + updatedJointAcceleration)) + { + log()->error("{} Cannot compute the inverse dynamics.", errorPrefix); + return false; + } + + return true; +} + +void RDE::SubModelDynamics::computeTotalTorqueFromContacts() +{ + totalTorqueFromContacts.setZero(); + + // Contribution of FT measurements + for (int idx = 0; idx < subModel.getNrOfFTSensor(); idx++) + { + wrench = (int)subModel.getFTSensor(idx).forceDirection * FTMap[subModel.getFTSensor(idx).name].array(); + + torqueFromContact = kinDynWrapper->getFTJacobian(subModel.getFTSensor(idx).name).block(0, 6, 6, subModel.getModel().getNrOfDOFs()).transpose() * wrench; + + totalTorqueFromContacts = totalTorqueFromContacts.array() + torqueFromContact.array(); + } + + // Contribution of unknown external contacts + for (int idx = 0; idx < subModel.getNrOfExternalContact(); idx++) + { + torqueFromContact = kinDynWrapper->getFTJacobian(subModel.getFTSensor(idx).name).block(0, 6, 6, subModel.getModel().getNrOfDOFs()).transpose() * FTMap[subModel.getExternalContact(idx)]; + + totalTorqueFromContacts = totalTorqueFromContacts.array() + torqueFromContact.array(); + } +} diff --git a/src/Estimators/src/UkfState.cpp b/src/Estimators/src/UkfState.cpp new file mode 100644 index 0000000000..c68eaf776e --- /dev/null +++ b/src/Estimators/src/UkfState.cpp @@ -0,0 +1,327 @@ +/** + * @file UkfState.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +using namespace BipedalLocomotion; +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +struct RDE::UkfState::Impl +{ + bool isInitialized{false}; + bool isFinalized{false}; + + Eigen::Vector3d gravity{0, 0, -Math::StandardAccelerationOfGravitation}; /**< Gravity vector. */ + + Eigen::MatrixXd covarianceQ; /**< Covariance matrix. */ + std::size_t stateSize; /**< Length of the state vector. */ + double dT; /**< Sampling time */ + + std::map> dynamicsList; /**< List of the dynamics composing the process model. */ + + System::VariablesHandler stateVariableHandler; /**< Variable handler describing the state vector. */ + + std::shared_ptr kinDynFullModel; /**< KinDynComputation object for the full model. */ + std::vector subModelList; /**< List of SubModel object describing the sub-models composing the full model. */ + std::vector> kinDynWrapperList; /**< List of SubModelKinDynWrapper objects containing kinematic and dynamic information specific of each sub-model. */ + + std::shared_ptr ukfInputProvider; /**< Provider containing the updated robot state. */ + UKFInput ukfInput; /**< Struct containing the inputs for the ukf populated by the ukfInputProvider. */ + + Eigen::VectorXd jointVelocityState; /**< Joint velocity compute by the ukf. */ + Eigen::VectorXd currentState; /**< State estimated in the previous step. */ + Eigen::VectorXd nextState; /**< Vector containing all the updated states. */ +}; + +RDE::UkfState::UkfState() +{ + m_pimpl = std::make_unique(); +} + +RDE::UkfState::~UkfState() = default; + +bool RDE::UkfState::initialize(std::weak_ptr handler) +{ + constexpr auto logPrefix = "[UkfState::initialize]"; + + auto ptr = handler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is not valid.", logPrefix); + return false; + } + + if (!ptr->getParameter("sampling_time", m_pimpl->dT)) + { + log()->error("{} Unable to find the `sampling_time` variable", logPrefix); + return false; + } + + m_pimpl->isInitialized = true; + + return true; +} + +bool RDE::UkfState::finalize(const System::VariablesHandler& handler) +{ + constexpr auto logPrefix = "[UkfState::finalize]"; + + if (!m_pimpl->isInitialized) + { + log()->error("{} Please call initialize() before finalize().", logPrefix); + return false; + } + + m_pimpl->isFinalized = false; + + m_pimpl->stateSize = 0; + + // finalize all the dynamics + for (auto& [name, dynamics] : m_pimpl->dynamicsList) + { + if(!dynamics->finalize(handler)) + { + log()->error("{} Error while finalizing the dynamics names {}", logPrefix, name); + return false; + } + + m_pimpl->stateSize += dynamics->size(); + } + + + // Set value of process covariance matrix + m_pimpl->covarianceQ.resize(m_pimpl->stateSize, m_pimpl->stateSize); + m_pimpl->covarianceQ.setZero(); + + for (auto& [name, dynamics] : m_pimpl->dynamicsList) + { + m_pimpl->covarianceQ.block(handler.getVariable(name).offset, handler.getVariable(name).offset, + handler.getVariable(name).size, handler.getVariable(name).size) = dynamics->getCovariance().asDiagonal(); + } + + m_pimpl->jointVelocityState.resize(m_pimpl->kinDynFullModel->model().getNrOfDOFs()); + + m_pimpl->currentState.resize(m_pimpl->stateSize); + m_pimpl->currentState.setZero(); + + m_pimpl->nextState.resize(m_pimpl->stateSize); + m_pimpl->nextState.setZero(); + + m_pimpl->isFinalized = true; + + return true; +} + +std::unique_ptr RDE::UkfState::build(std::weak_ptr handler, + std::shared_ptr kinDynFullModel, + const std::vector& subModelList, + const std::vector>& kinDynWrapperList) +{ + constexpr auto logPrefix = "[UkfState::build]"; + + auto ptr = handler.lock(); + if (ptr == nullptr) + { + log()->error("{} Invalid parameter handler.", logPrefix); + return nullptr; + } + + std::unique_ptr state = std::make_unique(); + + if (!state->initialize(ptr)) + { + log()->error("{} Unable to initialize the RobotDynamicsEstimator.", logPrefix); + return nullptr; + } + + std::vector dynamicsList; + if (!ptr->getParameter("dynamics_list", dynamicsList)) + { + log()->error("{} Unable to find the parameter 'dynamics_list'.", logPrefix); + return nullptr; + } + + if (kinDynFullModel == nullptr) + { + log()->error("{} The pointer to the `KinDynComputation` object is not valid.", logPrefix); + return nullptr; + } + + // Set KinDynComputation for the full model + state->m_pimpl->kinDynFullModel = kinDynFullModel; + + // Set the list of SubModel + state->m_pimpl->subModelList.reserve(subModelList.size()); + state->m_pimpl->subModelList = subModelList; + + // set the list of SubModelKinDynWrapper + state->m_pimpl->kinDynWrapperList.reserve(kinDynWrapperList.size()); + state->m_pimpl->kinDynWrapperList = kinDynWrapperList; + + // per each dynamics add variable to the variableHandler + // and add the dynamics to the list + for (const auto& dynamicsGroupName : dynamicsList) + { + auto dynamicsGroupTmp = ptr->getGroup(dynamicsGroupName).lock(); + if (dynamicsGroupTmp == nullptr) + { + log()->error("{} Unable to find the group '{}'.", logPrefix, dynamicsGroupName); + return nullptr; + } + + // add dT parameter since it is required by all the dynamics + auto dynamicsGroup = dynamicsGroupTmp->clone(); + dynamicsGroup->setParameter("sampling_time", state->m_pimpl->dT); + + // create variable handler + std::string dynamicsName; + std::vector covariances; + std::vector dynamicsElements; + if (!dynamicsGroup->getParameter("name", dynamicsName)) + { + log()->error("{} Unable to find the parameter 'name'.", logPrefix); + return nullptr; + } + if (!dynamicsGroup->getParameter("covariance", covariances)) + { + log()->error("{} Unable to find the parameter 'covariance'.", logPrefix); + return nullptr; + } + if (!dynamicsGroup->getParameter("elements", dynamicsElements)) + { + log()->error("{} Unable to find the parameter 'elements'.", logPrefix); + return nullptr; + } + state->m_pimpl->stateVariableHandler.addVariable(dynamicsName, covariances.size(), dynamicsElements); + + std::string dynamicModel; + if (!dynamicsGroup->getParameter("dynamic_model", dynamicModel)) + { + log()->error("{} Unable to find the parameter 'dynamic_model'.", logPrefix); + return nullptr; + } + + std::shared_ptr dynamicsInstance = RDE::DynamicsFactory::createInstance(dynamicModel); + if (dynamicsInstance == nullptr) + { + log()->error("{} The dynamic model '{}' has not been registered.", logPrefix, dynamicModel); + return nullptr; + } + + dynamicsInstance->setSubModels(subModelList, kinDynWrapperList); + + dynamicsInstance->initialize(dynamicsGroup); + + // add dynamics to the list + state->m_pimpl->dynamicsList.insert({dynamicsName, dynamicsInstance}); + } + + // finalize estimator + if (!state->finalize(state->m_pimpl->stateVariableHandler)) + { + log()->error("{} Unable to finalize the RobotDynamicsEstimator.", logPrefix); + return nullptr; + } + + return state; +} + +void RDE::UkfState::setUkfInputProvider(std::shared_ptr ukfInputProvider) +{ + m_pimpl->ukfInputProvider = ukfInputProvider; +} + +Eigen::MatrixXd RDE::UkfState::getNoiseCovarianceMatrix() +{ + return m_pimpl->covarianceQ; +} + +System::VariablesHandler RDE::UkfState::getStateVariableHandler() +{ + return m_pimpl->stateVariableHandler; +} + +void RDE::UkfState::propagate(const Eigen::Ref& cur_states, Eigen::Ref prop_states) +{ + constexpr auto logPrefix = "[UkfState::propagate]"; + + // Check that everything is initialized and set + if (!m_pimpl->isFinalized) + { + log()->error("{} The ukf state is not well initialized.", logPrefix); + throw std::runtime_error("Error"); + } + + if (m_pimpl->ukfInputProvider == nullptr) + { + log()->error("{} The ukf input provider is not set.", logPrefix); + throw std::runtime_error("Error"); + } + + // Get input of ukf from provider + m_pimpl->ukfInput = m_pimpl->ukfInputProvider->getOutput(); + + m_pimpl->currentState = cur_states; + + m_pimpl->jointVelocityState = m_pimpl->currentState.segment(m_pimpl->stateVariableHandler.getVariable("ds").offset, + m_pimpl->stateVariableHandler.getVariable("ds").size); + + // Update kindyn full model + m_pimpl->kinDynFullModel->setRobotState(m_pimpl->ukfInput.robotBasePose.transform(), + m_pimpl->ukfInput.robotJointPositions, + iDynTree::make_span(m_pimpl->ukfInput.robotBaseVelocity.data(), manif::SE3d::Tangent::DoF), + m_pimpl->jointVelocityState, + m_pimpl->gravity); + + // Update kindyn sub-models + for (int subModelIdx = 0; subModelIdx < m_pimpl->subModelList.size(); subModelIdx++) + { + m_pimpl->kinDynWrapperList[subModelIdx]->updateInternalKinDynState(); + } + + // TODO + // This could be parallelized + + // Update all the dynamics + for (auto& [name, dynamics] : m_pimpl->dynamicsList) + { + dynamics->setState(m_pimpl->currentState); + + dynamics->setInput(m_pimpl->ukfInput); + + if (!dynamics->update()) + { + log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, name); + throw std::runtime_error("Error"); + } + + m_pimpl->nextState.segment(m_pimpl->stateVariableHandler.getVariable(name).offset, + m_pimpl->stateVariableHandler.getVariable(name).size) = dynamics->getUpdatedVariable(); + } + + prop_states = m_pimpl->nextState; +} + +bfl::VectorDescription RDE::UkfState::getStateDescription() +{ + return bfl::VectorDescription(m_pimpl->stateSize); +} + +std::size_t RDE::UkfState::getStateSize() +{ + return m_pimpl->stateSize; +} diff --git a/src/Estimators/src/ZeroVelocityDynamics.cpp b/src/Estimators/src/ZeroVelocityDynamics.cpp new file mode 100644 index 0000000000..6a2a6c4431 --- /dev/null +++ b/src/Estimators/src/ZeroVelocityDynamics.cpp @@ -0,0 +1,169 @@ +/** + * @file ZeroVelocityDynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include + +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +bool RDE::ZeroVelocityDynamics::initialize(std::weak_ptr paramHandler) +{ + constexpr auto errorPrefix = "[ZeroVelocityDynamics::initialize]"; + + auto ptr = paramHandler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is not valid.", errorPrefix); + return false; + } + + // Set the state dynamics name + if (!ptr->getParameter("name", m_name)) + { + log()->error("{} Error while retrieving the name variable.", errorPrefix); + return false; + } + + // Set the state process covariance + if (!ptr->getParameter("covariance", m_covariances)) + { + log()->error("{} Error while retrieving the covariance variable.", errorPrefix); + return false; + } + + // Set the dynamic model type + if (!ptr->getParameter("dynamic_model", m_dynamicModel)) + { + log()->error("{} Error while retrieving the dynamic_model variable.", errorPrefix); + return false; + } + + // Set the list of elements if it exists + if (!ptr->getParameter("elements", m_elements)) + { + log()->info("{} Variable elements not found.", errorPrefix); + m_elements = {}; + } + + // Set the bias related variables if use_bias is true + if (!ptr->getParameter("use_bias", m_useBias)) + { + log()->info("{} Variable use_bias not found. Set to false by default.", errorPrefix); + } + else + { + m_useBias = true; + m_biasVariableName = m_name + "_bias"; + } + + m_description = "Zero velocity dynamics"; + + m_isInitialized = true; + + return true; +} + +bool RDE::ZeroVelocityDynamics::finalize(const System::VariablesHandler &stateVariableHandler) +{ + constexpr auto errorPrefix = "[ZeroVelocityDynamics::finalize]"; + + if (!m_isInitialized) + { + log()->error("{} Please initialize the dynamics before calling finalize.", errorPrefix); + return false; + } + + if (stateVariableHandler.getNumberOfVariables() == 0) + { + log()->error("{} The state variable handler is empty.", errorPrefix); + return false; + } + + m_stateVariableHandler = stateVariableHandler; + + if (!checkStateVariableHandler()) + { + log()->error("{} The state variable handler is not valid.", errorPrefix); + return false; + } + + m_size = m_covariances.size(); + + m_currentState.resize(m_size); + m_currentState.setZero(); + + m_updatedVariable.resize(m_size); + m_updatedVariable.setZero(); + + // Set the bias related variables if use_bias is true + if (m_useBias) + { + m_bias.resize(m_size); + m_bias.setZero(); + } + + return true; +} + +bool RDE::ZeroVelocityDynamics::setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) +{ + return true; +} + +bool RDE::ZeroVelocityDynamics::checkStateVariableHandler() +{ + constexpr auto errorPrefix = "[FrictionTorqueStateDynamics::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); + return false; + } + + if (m_useBias) + { + if (!m_stateVariableHandler.getVariable(m_biasVariableName).isValid()) + { + log()->error("{} The variable handler does not contain the expected state name {}.", errorPrefix, m_biasVariableName); + return false; + } + } + + return true; +} + +bool RDE::ZeroVelocityDynamics::update() +{ + if (m_useBias) + { + m_updatedVariable = m_currentState + m_bias; + } + else + { + m_updatedVariable = m_currentState; + } + + return true; +} + +void RDE::ZeroVelocityDynamics::setState(const Eigen::Ref ukfState) +{ + m_currentState = ukfState.segment(m_stateVariableHandler.getVariable(m_name).offset, + m_stateVariableHandler.getVariable(m_name).size); + + if (m_useBias) + { + m_bias = ukfState.segment(m_stateVariableHandler.getVariable(m_biasVariableName).offset, + m_stateVariableHandler.getVariable(m_biasVariableName).size); + } +} + +void RDE::ZeroVelocityDynamics::setInput(const UKFInput& ukfInput) +{ + return; +} diff --git a/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt b/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt index 5631deab95..1bd07a78ea 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt +++ b/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt @@ -23,6 +23,42 @@ if(FRAMEWORK_USE_icub-models AND FRAMEWORK_COMPILE_YarpImplementation) MANIF::manif ) + add_bipedal_test(NAME ZeroVelocityDynamics + SOURCES ZeroVelocityDynamicsTest.cpp + LINKS BipedalLocomotion::RobotDynamicsEstimator + BipedalLocomotion::ParametersHandler + BipedalLocomotion::ParametersHandlerYarpImplementation + icub-models::icub-models + Eigen3::Eigen + ) + + add_bipedal_test(NAME FrictionTorqueStateDynamics + SOURCES FrictionTorqueStateDynamicsTest.cpp + LINKS BipedalLocomotion::RobotDynamicsEstimator + BipedalLocomotion::ParametersHandler + BipedalLocomotion::ParametersHandlerYarpImplementation + BipedalLocomotion::System + icub-models::icub-models + Eigen3::Eigen + ) + + add_bipedal_test(NAME JointVelocityStateDynamics + SOURCES JointVelocityStateDynamicsTest.cpp + LINKS BipedalLocomotion::RobotDynamicsEstimator + BipedalLocomotion::ParametersHandler + BipedalLocomotion::ParametersHandlerYarpImplementation + BipedalLocomotion::System + icub-models::icub-models + Eigen3::Eigen + MANIF::manif + ) + + add_bipedal_test(NAME UkfState + SOURCES UkfStateTest.cpp + LINKS BipedalLocomotion::RobotDynamicsEstimator BipedalLocomotion::ManifConversions BipedalLocomotion::ParametersHandlerYarpImplementation + icub-models::icub-models + ) + include_directories(${CMAKE_CURRENT_BINARY_DIR}) configure_file("${CMAKE_CURRENT_SOURCE_DIR}/ConfigFolderPath.h.in" "${CMAKE_CURRENT_BINARY_DIR}/ConfigFolderPath.h" @ONLY) diff --git a/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp new file mode 100644 index 0000000000..fa9968c9de --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp @@ -0,0 +1,216 @@ +/** + * @file FrictionTorqueDynamicsTest.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; + +void createModelLoader(IParametersHandler::shared_ptr group, + iDynTree::ModelLoader& mdlLdr) +{ + // List of joints and fts to load the model + std::vector subModelList; + + const std::string modelPath = iCubModels::getModelFile("iCubGenova09"); + + std::vector jointList; + REQUIRE(group->getParameter("joint_list", jointList)); + + std::vector ftFramesList; + auto ftGroup = group->getGroup("FT").lock(); + REQUIRE(ftGroup->getParameter("frames", ftFramesList)); + + std::vector jointsAndFTs; + jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); +// jointsAndFTs.insert(jointsAndFTs.end(), ftFramesList.begin(), ftFramesList.end()); + + REQUIRE(mdlLdr.loadReducedModelFromFile(modelPath, jointsAndFTs)); +} + +void createSubModels(iDynTree::ModelLoader& mdlLdr, + std::shared_ptr kinDynFullModel, + IParametersHandler::shared_ptr group, + SubModelCreator& subModelCreator) +{ + subModelCreator.setModelAndSensors(mdlLdr.model(), mdlLdr.sensors()); + REQUIRE(subModelCreator.setKinDyn(kinDynFullModel)); + + REQUIRE(subModelCreator.createSubModels(group)); +} + +bool setStaticState(std::shared_ptr kinDyn) +{ + size_t dofs = kinDyn->getNrOfDegreesOfFreedom(); + iDynTree::Transform worldTbase; + Eigen::VectorXd baseVel(6); + Eigen::Vector3d gravity; + + Eigen::VectorXd qj(dofs), dqj(dofs), ddqj(dofs); + + worldTbase = iDynTree::Transform(iDynTree::Rotation::Identity(), iDynTree::Position::Zero()); + + Eigen::Matrix4d transform = toEigen(worldTbase.asHomogeneousTransform()); + + gravity.setZero(); + gravity(2) = - BipedalLocomotion::Math::StandardAccelerationOfGravitation; + + baseVel.setZero(); + + for (size_t dof = 0; dof < dofs; dof++) + { + qj(dof) = 0.0; + dqj(dof) = 0.0; + ddqj(dof) = 0.0; + } + + return kinDyn->setRobotState(transform, + iDynTree::make_span(qj.data(), qj.size()), + iDynTree::make_span(baseVel.data(), baseVel.size()), + iDynTree::make_span(dqj.data(), dqj.size()), + iDynTree::make_span(gravity.data(), gravity.size())); +} + +TEST_CASE("Friction Torque Dynamics") +{ + // Create parameter handler + auto parameterHandler = std::make_shared(); + + const std::string name = "tau_F"; + Eigen::VectorXd covariance(6); + covariance << 1e-3, 1e-3, 5e-3, 5e-3, 5e-3, 5e-3; + const std::string model = "FrictionTorqueStateDynamics"; + const std::vector elements = {"r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll"}; + Eigen::VectorXd k0(6); + k0 << 9.106, 5.03, 4.93, 12.88, 14.34, 1.12; + Eigen::VectorXd k1(6); + k1 << 200.0, 6.9, 200.0, 59.87, 200.0, 200.0; + Eigen::VectorXd k2(6); + k2 << 1.767, 5.64, 0.27, 2.0, 3.0, 0.0; + double dT = 0.01; + + parameterHandler->setParameter("name", name); + parameterHandler->setParameter("covariance", covariance); + parameterHandler->setParameter("dynamic_model", model); + parameterHandler->setParameter("elements", elements); + parameterHandler->setParameter("friction_k0", k0); + parameterHandler->setParameter("friction_k1", k1); + parameterHandler->setParameter("friction_k2", k2); + parameterHandler->setParameter("sampling_time", dT); + + // 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)); + + auto modelParamHandler = std::make_shared(); + auto emptyGroupNamesFrames = std::make_shared(); + std::vector emptyVectorString; + emptyGroupNamesFrames->setParameter("names", emptyVectorString); + emptyGroupNamesFrames->setParameter("frames", emptyVectorString); + REQUIRE(modelParamHandler->setGroup("FT", emptyGroupNamesFrames)); + REQUIRE(modelParamHandler->setGroup("ACCELEROMETER", emptyGroupNamesFrames)); + REQUIRE(modelParamHandler->setGroup("GYROSCOPE", emptyGroupNamesFrames)); + auto emptyGroupFrames = std::make_shared(); + emptyGroupFrames->setParameter("frames", emptyVectorString); + REQUIRE(modelParamHandler->setGroup("EXTERNAL_CONTACT", emptyGroupFrames)); + modelParamHandler->setParameter("joint_list", elements); + + iDynTree::ModelLoader modelLoader; + createModelLoader(modelParamHandler, modelLoader); + + auto kinDyn = std::make_shared(); + REQUIRE(kinDyn->loadRobotModel(modelLoader.model())); + REQUIRE(kinDyn->setFrameVelocityRepresentation(iDynTree::BODY_FIXED_REPRESENTATION)); + + REQUIRE(setStaticState(kinDyn)); + + SubModelCreator subModelCreator; + createSubModels(modelLoader, kinDyn, modelParamHandler, subModelCreator); + + std::vector> kinDynWrapperList; + const auto & subModelList = subModelCreator.getSubModelList(); + + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + kinDynWrapperList.emplace_back(std::make_shared()); + REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); + REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); + REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState()); + } + + FrictionTorqueStateDynamics tau_F_dynamics; + + REQUIRE(tau_F_dynamics.setSubModels(subModelList, kinDynWrapperList)); + REQUIRE(tau_F_dynamics.initialize(parameterHandler)); + REQUIRE(tau_F_dynamics.finalize(variableHandler)); + + manif::SE3d::Tangent robotBaseAcceleration; + robotBaseAcceleration.setZero(); + + Eigen::VectorXd robotJointAcceleration(kinDyn->model().getNrOfDOFs()); + robotJointAcceleration.setZero(); + + iDynTree::LinkNetExternalWrenches extWrench(kinDyn->model()); + extWrench.zero(); + + // Compute joint torques in static configuration from inverse dynamics on the full model + iDynTree::FreeFloatingGeneralizedTorques jointTorques(kinDyn->model()); + + kinDyn->inverseDynamics(iDynTree::make_span(robotBaseAcceleration.data(), + manif::SE3d::Tangent::DoF), + robotJointAcceleration, + extWrench, + jointTorques); + + Eigen::VectorXd state; + state.resize(variableHandler.getVariable("tau_F").offset + variableHandler.getVariable("tau_F").size + 1); + state.setZero(); + + int offset = variableHandler.getVariable("tau_m").offset; + int size = variableHandler.getVariable("tau_m").size; + for (int jointIndex = 0; jointIndex < size; jointIndex++) + { + state[offset+jointIndex] = jointTorques.jointTorques()[jointIndex]; + } + + // Test with state set to zero. + tau_F_dynamics.setState(state); + REQUIRE(tau_F_dynamics.update()); + for (int idx = 0; idx < tau_F_dynamics.getUpdatedVariable().size(); idx++) + { + REQUIRE(std::abs(tau_F_dynamics.getUpdatedVariable()(idx)) < 0.1); + } + + // Set random friction torque + Eigen::VectorXd currentStateTauF(covariance.size()); + for (int index = 0; index < currentStateTauF.size(); index++) + { + currentStateTauF(index) = GENERATE(take(1, random(-30, 30))); + } + + state.segment(variableHandler.getVariable("tau_F").offset, variableHandler.getVariable("tau_F").size) = currentStateTauF; + + tau_F_dynamics.setState(state); + + REQUIRE(tau_F_dynamics.update()); +} diff --git a/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp new file mode 100644 index 0000000000..28220ce965 --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp @@ -0,0 +1,263 @@ +/** + * @file FrictionTorqueDynamicsTest.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include +#include + +// iDynTree +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; + +IParametersHandler::shared_ptr loadParameterHandler() +{ + std::shared_ptr originalHandler = std::make_shared(); + IParametersHandler::shared_ptr parameterHandler = originalHandler; + + yarp::os::ResourceFinder& rf = yarp::os::ResourceFinder::getResourceFinderSingleton(); + rf.setDefaultConfigFile("/config/model.ini"); + + std::vector arguments = {" ", "--from ", getConfigPath()}; + + std::vector argv; + for (const auto& arg : arguments) + argv.push_back((char*)arg.data()); + argv.push_back(nullptr); + + rf.configure(argv.size() - 1, argv.data()); + + REQUIRE_FALSE(rf.isNull()); + parameterHandler->clear(); + REQUIRE(parameterHandler->isEmpty()); + originalHandler->set(rf); + + auto group = parameterHandler->getGroup("MODEL").lock(); + REQUIRE(group != nullptr); + + return group; +} + +void createModelLoader(IParametersHandler::shared_ptr group, + iDynTree::ModelLoader& mdlLdr) +{ + // List of joints and fts to load the model + std::vector subModelList; + + const std::string modelPath = iCubModels::getModelFile("iCubGenova09"); + + std::vector jointList; + REQUIRE(group->getParameter("joint_list", jointList)); + + std::vector ftFramesList; + auto ftGroup = group->getGroup("FT").lock(); + REQUIRE(ftGroup->getParameter("frames", ftFramesList)); + + std::vector jointsAndFTs; + jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); + jointsAndFTs.insert(jointsAndFTs.end(), ftFramesList.begin(), ftFramesList.end()); + + REQUIRE(mdlLdr.loadReducedModelFromFile(modelPath, jointsAndFTs)); +} + +void createSubModels(iDynTree::ModelLoader& mdlLdr, + std::shared_ptr kinDynFullModel, + IParametersHandler::shared_ptr group, + bool useFTSensors, + SubModelCreator& subModelCreator) +{ + subModelCreator.setModelAndSensors(mdlLdr.model(), mdlLdr.sensors()); + REQUIRE(subModelCreator.setKinDyn(kinDynFullModel)); + + if (useFTSensors) + { + REQUIRE(subModelCreator.createSubModels(group)); + } + else + { + auto groupEmpty = group->clone(); + groupEmpty->clear(); + + std::vector jointList; + group->getParameter("joint_list", jointList); + groupEmpty->setParameter("joint_list", jointList); + + std::shared_ptr emptySubGroup = groupEmpty->clone(); + emptySubGroup->clear(); + std::vector emptyVector; + emptySubGroup->setParameter("names", emptyVector); + emptySubGroup->setParameter("frames", emptyVector); + groupEmpty->setGroup("FT", emptySubGroup); + groupEmpty->setGroup("ACCELEROMETER", emptySubGroup); + groupEmpty->setGroup("GYROSCOPE", emptySubGroup); + groupEmpty->setGroup("EXTERNAL_CONTACT", emptySubGroup); + + REQUIRE(subModelCreator.createSubModels(groupEmpty)); + } +} + +bool setStaticState(std::shared_ptr kinDyn) +{ + size_t dofs = kinDyn->getNrOfDegreesOfFreedom(); + iDynTree::Transform worldTbase; + Eigen::VectorXd baseVel(6); + Eigen::Vector3d gravity; + + Eigen::VectorXd qj(dofs), dqj(dofs), ddqj(dofs); + + worldTbase = iDynTree::Transform(iDynTree::Rotation::Identity(), iDynTree::Position::Zero()); + + Eigen::Matrix4d transform = toEigen(worldTbase.asHomogeneousTransform()); + + gravity.setZero(); + gravity(2) = -BipedalLocomotion::Math::StandardAccelerationOfGravitation; + + baseVel.setZero(); + + for (size_t dof = 0; dof < dofs; dof++) + { + qj(dof) = 0.0; + dqj(dof) = 0.0; + ddqj(dof) = 0.0; + } + + return kinDyn->setRobotState(transform, + iDynTree::make_span(qj.data(), qj.size()), + iDynTree::make_span(baseVel.data(), baseVel.size()), + iDynTree::make_span(dqj.data(), dqj.size()), + iDynTree::make_span(gravity.data(), gravity.size())); +} + +TEST_CASE("Joint Velocity Dynamics") +{ + // Create parameter handler + auto parameterHandler = std::make_shared(); + + const std::string name = "ds"; + Eigen::VectorXd covariance(6); + covariance << 1e-3, 1e-3, 5e-3, 5e-3, 5e-3, 5e-3; + std::string model = "JointVelocityStateDynamics"; + const std::vector elements = {"r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll"}; + double dT = 0.01; + + parameterHandler->setParameter("name", name); + parameterHandler->setParameter("covariance", covariance); + parameterHandler->setParameter("dynamic_model", model); + parameterHandler->setParameter("elements", elements); + parameterHandler->setParameter("sampling_time", dT); + + // 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("r_leg_ft_sensor", 6)); + REQUIRE(variableHandler.addVariable("r_foot_front_ft_sensor", 6)); + REQUIRE(variableHandler.addVariable("r_foot_rear_ft_sensor", 6)); + REQUIRE(variableHandler.addVariable("r_leg_ft_sensor_bias", 6)); + REQUIRE(variableHandler.addVariable("r_foot_front_ft_sensor_bias", 6)); + REQUIRE(variableHandler.addVariable("r_foot_rear_ft_sensor_bias", 6)); + REQUIRE(variableHandler.addVariable("r_leg_ft_acc_bias", 3)); + REQUIRE(variableHandler.addVariable("r_foot_front_ft_acc_bias", 3)); + REQUIRE(variableHandler.addVariable("r_foot_rear_ft_acc_bias", 3)); + REQUIRE(variableHandler.addVariable("r_leg_ft_gyro_bias", 3)); + REQUIRE(variableHandler.addVariable("r_foot_front_ft_gyro_bias", 3)); + REQUIRE(variableHandler.addVariable("r_foot_rear_ft_gyro_bias", 3)); + + auto handlerFromConfig = loadParameterHandler(); + + iDynTree::ModelLoader modelLoader; + createModelLoader(handlerFromConfig, modelLoader); + + auto kinDyn = std::make_shared(); + REQUIRE(kinDyn->loadRobotModel(modelLoader.model())); + + REQUIRE(setStaticState(kinDyn)); + + bool useFT = true; + + SubModelCreator subModelCreatorWithFT; + createSubModels(modelLoader, kinDyn, handlerFromConfig, useFT, subModelCreatorWithFT); + + std::vector> kinDynWrapperListWithFT; + const auto & subModelListWithFT = subModelCreatorWithFT.getSubModelList(); + + for (int idx = 0; idx < subModelCreatorWithFT.getSubModelList().size(); idx++) + { + kinDynWrapperListWithFT.emplace_back(std::make_shared()); + REQUIRE(kinDynWrapperListWithFT.at(idx)->setKinDyn(kinDyn)); + REQUIRE(kinDynWrapperListWithFT.at(idx)->initialize(subModelListWithFT[idx])); + REQUIRE(kinDynWrapperListWithFT.at(idx)->updateInternalKinDynState()); + } + + JointVelocityStateDynamics dsSplit; + model = "JointVelocityStateDynamics"; + parameterHandler->setParameter("dynamic_model", model); + REQUIRE(dsSplit.setSubModels(subModelListWithFT, kinDynWrapperListWithFT)); + REQUIRE(dsSplit.initialize(parameterHandler)); + REQUIRE(dsSplit.finalize(variableHandler)); + + Eigen::VectorXd state; + state.resize(variableHandler.getVariable("r_foot_rear_ft_gyro_bias").offset + variableHandler.getVariable("r_foot_rear_ft_gyro_bias").size + 1); + state.setZero(); + + auto massSecondSubModel = subModelListWithFT.at(1).getModel().getTotalMass(); + state(variableHandler.getVariable("r_leg_ft_sensor").offset+2) = massSecondSubModel * BipedalLocomotion::Math::StandardAccelerationOfGravitation; + + dsSplit.setState(state); + + REQUIRE(dsSplit.update()); + + std::cout << "Updated state splitting the model" << std::endl; + std::cout << dsSplit.getUpdatedVariable() << std::endl; + + useFT = false; + + SubModelCreator subModelCreatorWithoutFT; + createSubModels(modelLoader, kinDyn, handlerFromConfig, useFT, subModelCreatorWithoutFT); + + std::vector> kinDynWrapperListWithoutFT; + const auto & subModelListWithoutFT = subModelCreatorWithoutFT.getSubModelList(); + + for (int idx = 0; idx < subModelCreatorWithoutFT.getSubModelList().size(); idx++) + { + kinDynWrapperListWithoutFT.emplace_back(std::make_shared()); + REQUIRE(kinDynWrapperListWithoutFT.at(idx)->setKinDyn(kinDyn)); + REQUIRE(kinDynWrapperListWithoutFT.at(idx)->initialize(subModelListWithoutFT[idx])); + REQUIRE(kinDynWrapperListWithoutFT.at(idx)->updateInternalKinDynState()); + } + + JointVelocityStateDynamics dsNotSplit; + model = "JointVelocityStateDynamics"; + parameterHandler->setParameter("dynamic_model", model); + REQUIRE(dsNotSplit.setSubModels(subModelListWithoutFT, kinDynWrapperListWithoutFT)); + REQUIRE(dsNotSplit.initialize(parameterHandler)); + REQUIRE(dsNotSplit.finalize(variableHandler)); + + dsNotSplit.setState(state); + + REQUIRE(dsNotSplit.update()); + + std::cout << "Updated state without splitting the model" << std::endl; + std::cout << dsNotSplit.getUpdatedVariable() << std::endl; +} diff --git a/src/Estimators/tests/RobotDynamicsEstimator/UkfStateTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/UkfStateTest.cpp new file mode 100644 index 0000000000..0597c83dbd --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/UkfStateTest.cpp @@ -0,0 +1,192 @@ +/** + * @file UkfStateTest.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include +#include + +//BLF +#include +#include +#include + +// iDynTree +#include +#include + +//LIBRARYTOTEST +#include +#include +#include +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +IParametersHandler::shared_ptr loadParameterHandler() +{ + std::shared_ptr originalHandler = std::make_shared(); + IParametersHandler::shared_ptr parameterHandler = originalHandler; + + yarp::os::ResourceFinder& rf = yarp::os::ResourceFinder::getResourceFinderSingleton(); + rf.setDefaultConfigFile("/config/config.ini"); + + std::vector arguments = {" ", "--from ", getConfigPath()}; + + std::vector argv; + for (const auto& arg : arguments) + argv.push_back((char*)arg.data()); + argv.push_back(nullptr); + + rf.configure(argv.size() - 1, argv.data()); + + REQUIRE_FALSE(rf.isNull()); + parameterHandler->clear(); + REQUIRE(parameterHandler->isEmpty()); + + originalHandler->set(rf); + + return parameterHandler; +} + +void createModelLoader(IParametersHandler::weak_ptr group, + iDynTree::ModelLoader& mdlLdr) +{ + // List of joints and fts to load the model + std::vector subModelList; + + const std::string modelPath = iCubModels::getModelFile("iCubGenova09"); + + std::vector jointList; + REQUIRE(group.lock()->getParameter("joint_list", jointList)); + + std::vector ftFramesList; + auto ftGroup = group.lock()->getGroup("FT").lock(); + REQUIRE(ftGroup->getParameter("frames", ftFramesList)); + + std::vector jointsAndFTs; + jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); + jointsAndFTs.insert(jointsAndFTs.end(), ftFramesList.begin(), ftFramesList.end()); + + REQUIRE(mdlLdr.loadReducedModelFromFile(modelPath, jointsAndFTs)); +} + +TEST_CASE("UkfState") +{ + auto parameterHandler = loadParameterHandler(); + + iDynTree::ModelLoader modelLoader; + auto modelHandler = parameterHandler->getGroup("MODEL").lock(); + + REQUIRE_FALSE(modelHandler == nullptr); + + createModelLoader(modelHandler, modelLoader); + + auto kinDyn = std::make_shared(); + REQUIRE(kinDyn->loadRobotModel(modelLoader.model())); + + std::vector, std::shared_ptr>> subModelMap; + + SubModelCreator subModelCreator; + subModelCreator.setModelAndSensors(kinDyn->model(), modelLoader.sensors()); + REQUIRE(subModelCreator.setKinDyn(kinDyn)); + + REQUIRE(subModelCreator.createSubModels(modelHandler)); + + std::vector> kinDynWrapperList; + + const auto & subModelList = subModelCreator.getSubModelList(); + + for (int idx = 0; idx < subModelList.size(); idx++) + { + kinDynWrapperList.emplace_back(std::make_shared()); + REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); + REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); + REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState()); + } + + // Build the UkfState + auto groupUKF = parameterHandler->getGroup("UKF").lock(); + REQUIRE_FALSE(groupUKF == nullptr); + + auto groupUKFStateTmp = groupUKF->getGroup("UKF_STATE").lock(); + REQUIRE_FALSE(groupUKFStateTmp == nullptr); + + auto groupUKFState = groupUKFStateTmp->clone(); + double dT; + REQUIRE(parameterHandler->getGroup("GENERAL").lock()->getParameter("sampling_time", dT)); + groupUKFState->setParameter("sampling_time", dT); + + std::unique_ptr stateModel = UkfState::build(groupUKFState, + kinDyn, + subModelList, + kinDynWrapperList); + + // Create an input for the ukf state + UKFInput input; + + // Define joint positions + Eigen::VectorXd jointPos; + jointPos.resize(kinDyn->model().getNrOfDOFs()); + jointPos.setZero(); + input.robotJointPositions = jointPos; + + // Define base pose + manif::SE3d basePose; + basePose.setIdentity(); + input.robotBasePose = basePose; + + // Define base velocity and acceleration + manif::SE3d::Tangent baseVelocity, baseAcceleration; + baseVelocity.setZero(); + baseAcceleration.setZero(); + input.robotBaseVelocity = baseVelocity; + input.robotBaseAcceleration = baseAcceleration; + + std::shared_ptr inputProvider = std::make_shared(); + + BipedalLocomotion::System::VariablesHandler stateHandler = stateModel->getStateVariableHandler(); + int stateSize = stateModel->getStateSize(); + + stateModel->setUkfInputProvider(inputProvider); + + Eigen::VectorXd currentState; + currentState.resize(stateSize); + currentState.setZero(); + + Eigen::VectorXd motorTorques; + motorTorques.resize(kinDyn->model().getNrOfDOFs()); + motorTorques << -1.6298, -1.10202, 0, -0.74, 0.0877, -0.00173; + Eigen::VectorXd wrenchFTtLeg; + wrenchFTtLeg.resize(6); + wrenchFTtLeg << 0, 0, 100.518, 0.748, 0.91, 0; + Eigen::VectorXd wrenchFTFootFront; + wrenchFTFootFront.resize(6); + wrenchFTFootFront << 0, 0, 1.761, -0.001, 0.0003, 0; + Eigen::VectorXd wrenchFTFootRear; + wrenchFTFootRear.resize(6); + wrenchFTFootRear << 0, 0, 1.752, 0.000876, 0.000649, 0; + + currentState.segment(stateHandler.getVariable("tau_m").offset, stateHandler.getVariable("tau_m").size) = motorTorques; + currentState.segment(stateHandler.getVariable("r_leg_ft_sensor").offset, stateHandler.getVariable("r_leg_ft_sensor").size) = wrenchFTtLeg; + currentState.segment(stateHandler.getVariable("r_foot_front_ft_sensor").offset, stateHandler.getVariable("r_foot_front_ft_sensor").size) = wrenchFTFootFront; + currentState.segment(stateHandler.getVariable("r_foot_rear_ft_sensor").offset, stateHandler.getVariable("r_foot_rear_ft_sensor").size) = wrenchFTFootRear; + + Eigen::VectorXd updatedState; + updatedState.resize(stateSize); + + REQUIRE(inputProvider->setInput(input)); + + stateModel->propagate(currentState, updatedState); + + for (int idx = 0; idx < updatedState.size(); idx++) + { + REQUIRE(std::abs(updatedState[idx]) < 200); + } +} diff --git a/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityDynamicsTest.cpp new file mode 100644 index 0000000000..667d39f73b --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityDynamicsTest.cpp @@ -0,0 +1,101 @@ +/** + * @file ZeroVelocityDynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include +#include + +#include +#include +#include + +#include + +using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; + +TEST_CASE("Zero Velocity Dynamics") +{ + auto parameterHandler = std::make_shared(); + + // Test without bias + const std::string name0 = "tau_m"; + Eigen::VectorXd covariance0(6); + covariance0 << 1e-3, 1e-3, 5e-3, 5e-3, 5e-3, 5e-3; + const std::string model0 = "ZeroVelocityDynamics"; + + parameterHandler->setParameter("name", name0); + parameterHandler->setParameter("covariance", covariance0); + parameterHandler->setParameter("dynamic_model", model0); + parameterHandler->setParameter("sampling_time", 0.01); + + // 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("r_leg_ft_sensor", sizeVariable)); + REQUIRE(variableHandler.addVariable("r_leg_ft_sensor_bias", sizeVariable)); + REQUIRE(variableHandler.addVariable("r_foot_front_ft_sensor", sizeVariable)); + + ZeroVelocityDynamics tau_m; + + REQUIRE(tau_m.initialize(parameterHandler)); + REQUIRE(tau_m.finalize(variableHandler)); + + Eigen::VectorXd currentState(sizeVariable); + for (int index = 0; index < sizeVariable; index++) + { + currentState(index) = GENERATE(take(1, random(-100, 100))); + } + + Eigen::VectorXd state; + state.resize(sizeVariable * variableHandler.getNumberOfVariables()); + state.setZero(); + state.segment(variableHandler.getVariable("tau_m").offset, variableHandler.getVariable("tau_m").size) = currentState; + + tau_m.setState(state); + + REQUIRE(tau_m.update()); + + Eigen::VectorXd nextState(covariance0.size()); + nextState = tau_m.getUpdatedVariable(); + + REQUIRE(currentState == nextState); + + // Test with bias + const std::string name1 = "r_leg_ft_sensor"; + Eigen::VectorXd covariance1(6); + covariance1 << 1e-7, 1e-2, 5e0, 5e-3, 5e-1, 5e-10; + const std::string model1 = "ZeroVelocityDynamics"; + const bool useBias = true; + + parameterHandler->clear(); + parameterHandler->setParameter("name", name1); + parameterHandler->setParameter("covariance", covariance1); + parameterHandler->setParameter("dynamic_model", model1); + parameterHandler->setParameter("sampling_time", 0.01); + parameterHandler->setParameter("use_bias", useBias); + + ZeroVelocityDynamics ft; + + REQUIRE(ft.initialize(parameterHandler)); + REQUIRE(ft.finalize(variableHandler)); + + state.segment(variableHandler.getVariable("r_leg_ft_sensor_bias").offset, variableHandler.getVariable("r_leg_ft_sensor_bias").size) = currentState; + + ft.setState(state); + REQUIRE(ft.update()); + + Eigen::VectorXd updatedVariable = ft.getUpdatedVariable(); + Eigen::VectorXd ftPre = state.segment(variableHandler.getVariable("r_leg_ft_sensor").offset, variableHandler.getVariable("r_leg_ft_sensor").size); + REQUIRE(std::abs(((currentState.array() + ftPre.array()) - updatedVariable.array()).array().sum()) == 0.0); +} diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/config.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/config.ini index 01410fd898..fb6521d62e 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/config/config.ini +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/config.ini @@ -4,3 +4,4 @@ sampling_time 0.01 [include MODEL "model.ini"] +[include UKF "ukf.ini"] diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf.ini new file mode 100644 index 0000000000..a9b0ac35b5 --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf.ini @@ -0,0 +1,4 @@ +[include UKF_STATE "ukf/ukf_state.ini"] + +[include UKF_MEASUREMENT "ukf/ukf_measurement.ini"] + diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini new file mode 100644 index 0000000000..fcd7b21691 --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini @@ -0,0 +1,46 @@ +# The state list should match the group names, otherwise the states are not loaded in the ukf model +measurement_list ("JOINT_VELOCITY", + "MOTOR_CURRENT", + "FT", + "ACCELEROMETER", + "GYROSCOPE") + +[JOINT_VELOCITY] +name ("ds") +elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") +covariance (1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4) +dynamic_model "LinearModel" + +[MOTOR_CURRENT] +name ("i_m") +elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") +covariance (7.5e-4, 7.6e-4, 1.6e-3, 1.4e-3, 1.3e-4, 7.4e-4) +dynamic_model "MotorCurrentModel" + +[FT] +name ("r_leg_ft_sensor", "r_foot_front_ft_sensor", "r_foot_rear_ft_sensor") +elements ("fx", "fy", "fz", "mx", "my", "mz") +covariance (1e-2, 1e-2, 1e-2, 1e-3, 1e-3, 1e-3, + 1e-2, 1e-2, 1e-2, 1e-5, 1e-5, 1e-5, + 1e-2, 1e-2, 1e-2, 1e-5, 1e-5, 1e-5) +use_bias 1 +dynamic_model "LinearModel" + +[ACCELEROMETER] +name ("r_leg_ft_acc", "r_foot_front_ft_acc", "r_foot_rear_ft_acc") +elements ("x", "y", "z") +covariance (2.3e-3, 1.9e-3, 3.1e-3, + 2.7e-3, 1.99e-3, 2.78e-3, + 2.5e-3, 2.3e-3, 3e-3) +use_bias 1 +dynamic_model "AccelerometerModel" + +[GYROSCOPE] +name ("r_leg_ft_gyro", "r_foot_front_ft_gyro", "r_foot_rear_ft_gyro") +elements ("x", "y", "z") +covariance (0.000000283, 0.0127, 0.0000001, + 4.1e-4, 3.3e-4, 3.2e-4, + 4.9e-4, 4.2e-4, 3.3e-4) +use_bias 1 +dynamic_model "GyroscopeModel" + diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini new file mode 100644 index 0000000000..3d94cb90e9 --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini @@ -0,0 +1,98 @@ +dynamics_list ("JOINT_VELOCITY", "MOTOR_TORQUE", "FRICTION_TORQUE", "RIGHT_LEG_FT", "RIGHT_FOOT_FRONT_FT", "RIGHT_FOOT_REAR_FT", + "RIGHT_LEG_FT_BIAS", "RIGHT_FOOT_FRONT_FT_BIAS", "RIGHT_FOOT_REAR_FT_BIAS", "RIGHT_LEG_ACC_BIAS", + "RIGHT_FOOT_FRONT_ACC_BIAS", "RIGHT_FOOT_REAR_ACC_BIAS", "RIGHT_LEG_GYRO_BIAS", "RIGHT_FOOT_FRONT_GYRO_BIAS", + "RIGHT_FOOT_REAR_GYRO_BIAS") + +[JOINT_VELOCITY] +name "ds" +elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") +covariance (1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3) +dynamic_model "JointVelocityStateDynamics" + +[MOTOR_TORQUE] +name "tau_m" +elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") +covariance (1e-3, 1e-3, 5e-3, 5e-3, 5e-3, 5e-3) +dynamic_model "ZeroVelocityDynamics" + +[FRICTION_TORQUE] +name "tau_F" +elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") +covariance (1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-1) +dynamic_model "FrictionTorqueStateDynamics" +friction_k0 (9.106, 5.03, 4.93, 12.88, 14.34, 1.12) +friction_k1 (200.0, 6.9, 200.0, 59.87, 200.0, 200.0) +friction_k2 (1.767, 5.64, 0.27, 2.0, 3.0, 0.0) + +[RIGHT_LEG_FT] +name "r_leg_ft_sensor" +elements ("fx", "fy", "fz", "mx", "my", "mz") +covariance (1e-3, 1e-3, 1e-3, 1e-4, 1e-4, 1e-4) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_FOOT_FRONT_FT] +name "r_foot_front_ft_sensor" +elements ("fx", "fy", "fz", "mx", "my", "mz") +covariance (1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_FOOT_REAR_FT] +name "r_foot_rear_ft_sensor" +elements ("fx", "fy", "fz", "mx", "my", "mz") +covariance (1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_LEG_FT_BIAS] +name "r_leg_ft_sensor_bias" +elements ("fx", "fy", "fz", "mx", "my", "mz") +covariance (1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_FOOT_FRONT_FT_BIAS] +name "r_foot_front_ft_sensor_bias" +elements ("fx", "fy", "fz", "mx", "my", "mz") +covariance (1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_FOOT_REAR_FT_BIAS] +name "r_foot_rear_ft_sensor_bias" +elements ("fx", "fy", "fz", "mx", "my", "mz") +covariance (1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_LEG_ACC_BIAS] +name "r_leg_ft_acc_bias" +elements ("x", "y", "z") +covariance (7.9e-5, 1.9e-5, 4.4e-5) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_FOOT_FRONT_ACC_BIAS] +name "r_foot_front_ft_acc_bias" +elements ("x", "y", "z") +covariance (7.4e-5, 1.2e-5, 4.4e-5) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_FOOT_REAR_ACC_BIAS] +name "r_foot_rear_ft_acc_bias" +elements ("x", "y", "z") +covariance (5.5e-5, 1.1e-5, 1.7e-5) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_LEG_GYRO_BIAS] +name "r_leg_ft_gyro_bias" +elements ("x", "y", "z") +covariance (4e-4, 4.7e-4, 4.7e-4) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_FOOT_FRONT_GYRO_BIAS] +name "r_foot_front_ft_gyro_bias" +elements ("x", "y", "z") +covariance (1.3e-2, 9.9e-3, 9e-3) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_FOOT_REAR_GYRO_BIAS] +name "r_foot_rear_ft_gyro_bias" +elements ("x", "y", "z") +covariance (8.2e-8, 1e-2, 9.3e-3) +dynamic_model "ZeroVelocityDynamics" + From 81a6abfbff371f79d4b2a2db8ea04ba5044a24d0 Mon Sep 17 00:00:00 2001 From: Ines Date: Thu, 2 Mar 2023 12:54:48 +0100 Subject: [PATCH 02/13] Update CHANGELOG --- CHANGELOG.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0ce6aec3f6..b77688cf56 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,7 @@ All notable changes to this project are documented in this file. - Add the support of `std::chrono` in The text logging (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) - Add the possibility to retrieve and set duration from the `IParametersHandler` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) - Add the possibility to update the contact list in the swing foot trajectory planner (https://github.com/ami-iit/bipedal-locomotion-framework/pull/637) +- Implement state dynamic models needed to define an UKF process model (https://github.com/ami-iit/bipedal-locomotion-framework/pull/615) ### Changed - Update the `IK tutorial` to use `QPInverseKinematics::build` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/621) @@ -26,7 +27,7 @@ All notable changes to this project are documented in this file. ### Fixed - Return an error if an invalid `KinDynComputations` object is passed to `QPInverseKinematics::build()` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/622) -- Fix `QPTSIF` documentation (https://github.com/ami-iit/bipedal-locomotion-framework/pull/634) +- Fix `QPTSID` documentation (https://github.com/ami-iit/bipedal-locomotion-framework/pull/634) - Fix error messages in `QPTSID` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/639) ## [0.12.0] - 2023-03-07 From c5ea236997357426708e6aaba99bafebeb5f4a21 Mon Sep 17 00:00:00 2001 From: Ines Date: Fri, 24 Mar 2023 13:24:58 +0100 Subject: [PATCH 03/13] Implement MotorCurrentMeasurementDynamics, AccelerometerMeasurementDynamics, GyroscopeMeasurementDynamics, UkfMeasurement classes and tests. --- CHANGELOG.md.orig | 388 ++++++++++++++++++ src/Estimators/CMakeLists.txt | 8 +- .../AccelerometerMeasurementDynamics.h | 135 ++++++ .../RobotDynamicsEstimator/Dynamics.h | 2 +- .../GyroscopeMeasurementDynamics.h | 126 ++++++ .../MotorCurrentMeasurementDynamics.h | 117 ++++++ .../RobotDynamicsEstimator/SubModel.h | 46 +++ .../RobotDynamicsEstimator/UkfMeasurement.h | 240 +++++++++++ .../ZeroVelocityDynamics.h | 2 +- .../src/AccelerometerMeasurementDynamics.cpp | 338 +++++++++++++++ .../src/GyroscopeMeasurementDynamics.cpp | 238 +++++++++++ .../src/MotorCurrentMeasurementDynamics.cpp | 149 +++++++ src/Estimators/src/SubModel.cpp | 85 ++++ src/Estimators/src/UkfMeasurement.cpp | 383 +++++++++++++++++ src/Estimators/src/UkfState.cpp | 4 +- .../AccelerometerMeasurementDynamicsTest.cpp | 229 +++++++++++ .../RobotDynamicsEstimator/CMakeLists.txt | 28 ++ .../FrictionTorqueStateDynamicsTest.cpp | 1 - .../GyroscopeMeasurementDynamicsTest.cpp | 207 ++++++++++ .../UkfMeasurementTest.cpp | 252 ++++++++++++ .../config/ukf/ukf_measurement.ini | 81 ++-- 21 files changed, 3023 insertions(+), 36 deletions(-) create mode 100644 CHANGELOG.md.orig create mode 100644 src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h create mode 100644 src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/GyroscopeMeasurementDynamics.h create mode 100644 src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/MotorCurrentMeasurementDynamics.h create mode 100644 src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfMeasurement.h create mode 100644 src/Estimators/src/AccelerometerMeasurementDynamics.cpp create mode 100644 src/Estimators/src/GyroscopeMeasurementDynamics.cpp create mode 100644 src/Estimators/src/MotorCurrentMeasurementDynamics.cpp create mode 100644 src/Estimators/src/UkfMeasurement.cpp create mode 100644 src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp create mode 100644 src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp create mode 100644 src/Estimators/tests/RobotDynamicsEstimator/UkfMeasurementTest.cpp diff --git a/CHANGELOG.md.orig b/CHANGELOG.md.orig new file mode 100644 index 0000000000..e94747b297 --- /dev/null +++ b/CHANGELOG.md.orig @@ -0,0 +1,388 @@ +# Changelog +All notable changes to this project are documented in this file. + +## [unreleased] +### Added +- Implement the `DiscreteGeometryContact` in Contacts component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/626) +- Implement the `SchmittTrigger` in component `Math` and the associated python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/624) +- Add the support of `std::chrono` in The text logging (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) +- Add the possibility to retrieve and set duration from the `IParametersHandler` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) +- Add the possibility to update the contact list in the swing foot trajectory planner (https://github.com/ami-iit/bipedal-locomotion-framework/pull/637) +- Implement state dynamic models needed to define an UKF process model (https://github.com/ami-iit/bipedal-locomotion-framework/pull/615) + +### Changed +- Update the `IK tutorial` to use `QPInverseKinematics::build` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/621) +- Handle case where no FT sensors are specified to split the model (https://github.com/ami-iit/bipedal-locomotion-framework/pull/625) +- General restructure of the `ContactDetector`and the derived classes (`SchmittTriggerDetector` and `FixedFootDetector`) (https://github.com/ami-iit/bipedal-locomotion-framework/pull/624) + Thanks to this refactory the `FixedFootDetector` usage becomes similar to the others `advanceable`. + Indeed now `FixedFootDetector::advace()` considers the input set by the user and provides the corresponding output. + ⚠️ Even if this modification do not break the API the user may notice some strange behavior if `advance` was called after getting the output of the detector. +- Restructure the `Contacts` component to handle time with `std::chrono::nanoseconds` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) +- Restructure the `Planners` component to handle time with `std::chrono::nanoseconds` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) +- Restructure the `FloatingBaseEstimator` component to handle time with `std::chrono::nanoseconds`(https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) +- Update the `blf-position-tracking` to handle time with `std::chrono::nanoseconds` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) +- Update the python bindings to consider the time with `std::chrono::nanoseconds` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) +- Robustify SubModelCreator and SubModelKinDynWrapper tests (https://github.com/ami-iit/bipedal-locomotion-framework/pull/631) +- `SwingFootTrajectoryPlanner::advance()` must be called before getting the output (https://github.com/ami-iit/bipedal-locomotion-framework/pull/637) + +### Fixed +- Return an error if an invalid `KinDynComputations` object is passed to `QPInverseKinematics::build()` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/622) +- Fix `QPTSID` documentation (https://github.com/ami-iit/bipedal-locomotion-framework/pull/634) +<<<<<<< HEAD +- Fix error messages in `QPTSID` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/639) +======= +>>>>>>> Update CHANGELOG + +## [0.12.0] - 2023-03-07 +### Added +- Add the possibility to attach all the multiple analog sensor clients (https://github.com/ami-iit/bipedal-locomotion-framework/pull/569) +- Add a tutorial for the inverse kinematics (https://github.com/ami-iit/bipedal-locomotion-framework/pull/596) +- Implement the ROS2 sink for the `TextLogging` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/587) +- Implement the `QPFixedBaseInverseKinematics` in the `IK` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/599) +- 🤖 [ergoCubSN000] Add configuration files for the YarpRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/600) +- Add functions to split a model in a set of submodels in the Estimator component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/604) +- Add the possibity to call the advanceable capabilities of the `QuinticSpline` from the python (https://github.com/ami-iit/bipedal-locomotion-framework/pull/609) +- Implement the `CubicSpline` python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/609) +- Implement the python bindings for the iDynTree to manif conversions (https://github.com/ami-iit/bipedal-locomotion-framework/pull/610) +- Implement `blf-balancing-position-control` application (https://github.com/ami-iit/bipedal-locomotion-framework/pull/611) +- Implement the python bindings for `YarpTextLogging` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/611) +- Add SubModelKinDynWrapper class to handle the `KinDynComputation` object of a sub-model (https://github.com/ami-iit/bipedal-locomotion-framework/pull/605) +- Implement the `JointLimitsTask` for the IK (https://github.com/ami-iit/bipedal-locomotion-framework/pull/603) +- Add the possibility to programmatically build an IK problem from a configuration file (https://github.com/ami-iit/bipedal-locomotion-framework/pull/614, https://github.com/ami-iit/bipedal-locomotion-framework/pull/619) + +### Changed +- Ask for `toml++ v3.0.1` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/581) +- The YarpRobotLogger will now automatically connect to the exogenous signal port if available (https://github.com/ami-iit/bipedal-locomotion-framework/pull/570/) +- 🤖 [iCubGenova09] Add the left and right hands skin (raw and filtered) data acquisition (https://github.com/ami-iit/bipedal-locomotion-framework/pull/570/) +- Add informative prints `YarpSensorBridge::Impl` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/569) +- The minimum version of iDynTree now supported is iDynTree 4.3.0 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/588). +- Allow using the `iDynTree swig` bindings in `QPFixedBaseTSID` for the kindyncomputation object (https://github.com/ami-iit/bipedal-locomotion-framework/pull/599) +- Add the possibility to customize the video codec in the `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/607) + +### Fix +- Return an invalid `PolyDriverDescriptor` if `description` is not found in `constructMultipleAnalogSensorsRemapper()` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/569) +- Fix compatibility with OpenCV 4.7.0 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/589) +- Fix `attachRemappedRemoteControlBoard` in `YarpSensorBridge` when the `RemoteControlBoard` is not the first polydriver in the polydriverlist (https://github.com/ami-iit/bipedal-locomotion-framework/pull/608) +- Fix race condition in System::ClockBuilder (https://github.com/ami-iit/bipedal-locomotion-framework/pull/618) + +## [0.11.1] - 2022-12-19 +### Fix +- Fix the compilation of the `YarpRobotLoggerDevice` in `Windows` and `macOS` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/580) + +## [0.11.0] - 2022-12-17 +### Added +- Log the status of the system in `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/571) +- Add the `ROS2` implementation for Clock class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/575) + +### Changed +- YARP devices are now enabled by default if YARP is found (https://github.com/ami-iit/bipedal-locomotion-framework/pull/576). +- Restructure the python bindings to support _official_ `iDynTree` bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/578) +- Remove _unofficial_ `iDynTree` bindings based on pybind11 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/578) + +### Fix +- Fix compatibility with YARP 3.8 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/577). + +## [0.10.0] - 2022-09-23 +### Added +- Add the possibility to set the exogenous feedback for the `IK::SE3Task` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/567) +- Implement `RobotInterface::constructMultipleAnalogSensorsClient()` and `RobotInterface::constructMultipleAnalogsensorsRemapper()` methods (https://github.com/ami-iit/bipedal-locomotion-framework/pull/568) + +### Changed +- Add the possibility to log only a subset of text logging ports in `YarpRobotLogger` device (https://github.com/ami-iit/bipedal-locomotion-framework/pull/561) +- Accept boolean as integer while getting an element from searchable in `YarpUtilities` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/566) + +### Fix +- Fix typo in the `RobotInterface::constructGenericSensorClient()` documentation (https://github.com/ami-iit/bipedal-locomotion-framework/pull/568) +- Fix compatibility with qhull installed by vcpkg `2022.07.25` and robotology-superbuild-dependencies-vcpkg `0.10.1` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/565). + +## [0.9.0] - 2022-09-09 +### Added +- Implement the `MultiStateWeightProvider` in `ContinuousDynamicalSystem` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/555) +- Implement `PortInput` and `PortOutput`in `System` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/555) +- Implement `toManifTwist` in `Conversions` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/557) +- Add the default value for the desired spatial and angular velocity to the `IK::SO3Task` and `IK::SE3Task` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/557) +- Implement the `IK::R3Task` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/559) + +### Changed +- Now `Advanceable` inherits from `PortInput` and `PortOutput` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/555) + +### Fix +- Fix the dependency required to compile the YarpRobotLogger device (https://github.com/ami-iit/bipedal-locomotion-framework/pull/554) +- Fix the compatibility with fmt v9.0.0 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/556) + +## [0.8.0] - 2022-07-29 +### Added +- Add the possibility to log the YarpTextLogging in the YarpRobotLogger (https://github.com/ami-iit/bipedal-locomotion-framework/pull/541) +- Enable the logging FTs and IMU logging of iCubGenova09 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/546/) +### Changed +- Ported `YarpLoggerDevice` to `robometry`(https://github.com/ami-iit/bipedal-locomotion-framework/pull/533) +- bipedal locomotion framework now depends on YARP 3.7.0 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/541) +### Fix +- Avoid to use deprecated function cv::aruco::drawAxis in ArucoDetector to fix compilation with OpenCV 4.6.0 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/552) + +## [0.7.0] - 2022-06-21 +### Added +- Implement the python bindings for the clock machinery and for the yarp clock (https://github.com/ami-iit/bipedal-locomotion-framework/pull/500) +- Implement the `IWeightProvider` interface and the `ConstantWeightProvider` class in the System component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/506) +- Add installation of pip metadata files for when blf python bindings are installed only via CMake (https://github.com/ami-iit/bipedal-locomotion-framework/pull/508) +- Implement the python bindings for the VectorsCollection message and the associated buffered port (https://github.com/ami-iit/bipedal-locomotion-framework/pull/511) +- Implement the `VectorsCollectionWrapper` device for collection of arbitrary vector ports (https://github.com/ami-iit/bipedal-locomotion-framework/pull/512) +- Add reading of right upper leg FT for `iCubGenova09` and associated cartesian wrench in `YarpRobotLoggerDevice` configuration files (https://github.com/ami-iit/bipedal-locomotion-framework/pull/513) +- Add reading of right and left arms FT for `iCubGenova09` in `YarpRobotLoggerDevice` configuration files (https://github.com/ami-iit/bipedal-locomotion-framework/pull/515) +- Add reading of right and left arms and right upper leg FTs and cartesian wrenches for `iCubGazeboV3` in `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/525) +- Add the possibility to retrieve the temperature sensor from `SensorBridge` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/519) +- Add the possibility to set only the velocity in `CubicSpline::setInitialConditions` and `CubicSpline::setFinalConditions` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/528) +- Implement the python bindings for the `ContinuousDynamicalSystem` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/532) +- Add the possibility to log the video in the `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/516) + +### Changed +- An error it will be returned if the user tries to change the clock type once the `clock()` has been already called once (https://github.com/ami-iit/bipedal-locomotion-framework/pull/500) +- Log the arms external wrenches on the YarpRobotLogger for iCubGenova09 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/502) +- IK and TSID now uses the weight provider to specify the weight associated to a task (https://github.com/ami-iit/bipedal-locomotion-framework/pull/506) +- The `Planners`, `System`, `RobotInterface` and `YarpImplementation` components are no more mandatory to compile the python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/511) +- Reorganize the multiple FT sensor and external wrench files into a single file in the YarpRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/525) +- Save the robot name and the names of the channel's elements in the YarpRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/522) +- Use icub-models to get the urdf models for the tests (https://github.com/ami-iit/bipedal-locomotion-framework/pull/526) +- The FT sensor are now considered as `multianalogsensor` in `YarpSensorBridge` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/519) +- Make `YarpRobotLogger` compatible with `yarp-telemetry` v0.5.1 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/535) +- Set for `yarp-telemetry` minimum version to v0.5.1 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/535) +- Make `YarpCameraBridge::getColorImage()` and `YarpCameraBridge::getDepthImage()` thread safe (https://github.com/ami-iit/bipedal-locomotion-framework/pull/516) +- Deprecate `YarpCameraBridge::get()` in favor of `YarpCameraBridge::getMetaData()` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/516) +- Move from LGPL to BSD3 license (https://github.com/ami-iit/bipedal-locomotion-framework/pull/550) + +### Fix +- Remove outdated includes in YarpRobotLoggerDevice.cpp (https://github.com/ami-iit/bipedal-locomotion-framework/pull/502) + +## [0.6.0] - 2022-01-10 +### Added +- Add the reading of the orientation of the head IMU in `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/471) +- Add the possibility to change the weight in TSID/IK (https://github.com/ami-iit/bipedal-locomotion-framework/pull/475) +- Implement a `FirstOrderSmoother` class in `ContinuousDynamicalSystem` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/476) +- Implement `getIntegrationStep` in `FixedIntegration` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/476) +- Add the possibility to create custom `LinearTasks` in python (https://github.com/ami-iit/bipedal-locomotion-framework/pull/480) +- Implement the possibility to compute the residual terms in the `LinearTask` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/483) +- Define `VectorsCollection` message in `YarpUtilities` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/483) +- Add the reading of the joint and motor acceleration in `YarpSensorBridge` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/492) + +### Changed +- Use yarp clock instead of system clock in `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/473) +- Reduce code duplication in python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/484) +- Use `TextLogger` in `YarpRobotLoggerDevice` instead of `yarp` commands (https://github.com/ami-iit/bipedal-locomotion-framework/pull/486) +- Ask for `osqp-eigen 0.6.4.100`(https://github.com/ami-iit/bipedal-locomotion-framework/pull/490) +- Use enum underlying type to convert `TextLogging` verbosity level to `spdlog` verbosity level (https://github.com/ami-iit/bipedal-locomotion-framework/pull/495) +- `yarp-telemetry` is now a dependency of the `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/487) +- Fix deprecated `YARP` functions in `YarpUtilities` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/491) + +### Fix +- Fix the population of the jointAccelerations and baseAcceleration variables in QPTSID (https://github.com/ami-iit/bipedal-locomotion-framework/pull/478) +- Fix the documentation in the `Advanceable` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/476) +- Add virtual destrutors in `System::Sink`, `System::Source`, `System::LinearTask`, +`System::ITaskControlMode`, `TSID::TSIDLinearTask` and `IK::IKLinearTask` classes (https://github.com/ami-iit/bipedal-locomotion-framework/pull/480) +- The joint torques is now correctly retrieved in QPTSID class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/482) +- The motor velocity and positions are now returned in rad/s and rad (https://github.com/ami-iit/bipedal-locomotion-framework/pull/489) +- Fix `YarpRobotLoggerDevice` documentation (https://github.com/ami-iit/bipedal-locomotion-framework/pull/472) + +## [0.5.0] - 2021-11-30 +### Added +- Implement Python bindings for the TSID component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/428) +- Add the possibility to set the name of each element of a variable stored in the variables handler (https://github.com/ami-iit/bipedal-locomotion-framework/pull/429) +- Develop the python bindings for toml implementation of the parameters handler (https://github.com/ami-iit/bipedal-locomotion-framework/pull/432) +- Implement the VariableRegularizationTask in TSID (https://github.com/ami-iit/bipedal-locomotion-framework/pull/431) +- Implement `create_tsid` utility function for the python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/433) +- Implement the `AngularMomentumTask` in the `TSID` component and the associated python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/436) +- Implement `QPTSID::toString` method and the associated python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/440) +- Implement `ContactWrench` python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/441) +- Implement AngularMomentum task in the IK component and the associated bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/443) +- Implement `create_ik` utility function for the python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/444) +- Add the possibility to set the task controller mode for the SE3Task in the TSID component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/445) +- Expose the `ITaskControlMode` class in the python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/445) +- Add the possibility to enable/disable the joints and motors state logging in the `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/453) +- Implement `QPInverseKinematics::toString` method and the associated python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/461) +- Add the cartesian wrenches logging in `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/447) +- Implement the python bindings for the manif conversions methods (https://github.com/ami-iit/bipedal-locomotion-framework/pull/465) + +### Changed +- Inherits all the `Eigen::Matrix` constructors in the `Wrenchd` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/441) +- Bump the minimum `cmake` version to `3.16.0` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/468) + +### Fix +- Fix Analog FT Sensor reading in `YarpSensorBridgeImpl` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/459) +- Fix config files in `YarpRobotLoggerDevice` for iCub3 head IMU reading (https://github.com/ami-iit/bipedal-locomotion-framework/pull/467) + +## [0.4.0] - 2021-10-15 +### Added +- Implement `AdvanceableRunner::isRunning()` method (https://github.com/ami-iit/bipedal-locomotion-framework/pull/395) +- Implement `ContactPhaseList::getPresentPhase()` method (https://github.com/ami-iit/bipedal-locomotion-framework/pull/396) +- Add a synchronization mechanism for the `AdvanceableRunner` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/403) +- Add the possibility to use spdlog with YARP (https://github.com/ami-iit/bipedal-locomotion-framework/pull/408) +- Add new Advanceable exposing `UnicyclePlanner` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/320) + +### Changed +- Add `name` parameter to the `AdvanceableRunner` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/406) +- Set the required `spdlog` version in the cmake file (https://github.com/ami-iit/bipedal-locomotion-framework/pull/415) +- Add features to FTIMULoggerDevice and rename it in YarpRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/405) + +### Fix +- Fix missing components dependencies in the `CMake` machinery (https://github.com/ami-iit/bipedal-locomotion-framework/pull/414) +- Fixed missing include in `FloatingBaseEstimatorIO.h` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/417) + +## [0.3.0] - 2021-08-12 +### Added +- Implement `CubicSpline` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/344) +- Implement `PWM` control in RobotControl class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/346) +- Implement `ContactWrenchCone` class in Math component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/352) +- Implement `skew` function in Math component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/352) +- Implement `QPTSID` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/366) +- Implement motor pwm, motor encoders, wbd joint torque estimates, pid reading in `YarpSensorBridge`(https://github.com/ami-iit/bipedal-locomotion-framework/pull/359). +- Implement FeasibleContactWrenchTask for TSID component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/369). +- Implement python bindings for QPInverseKinematics class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/303) +- Implement `ControlTask` in for System component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/373). +- Allow changing the log verbosity (https://github.com/ami-iit/bipedal-locomotion-framework/pull/385) +- Implement the CoMZMP controller (https://github.com/ami-iit/bipedal-locomotion-framework/pull/387) + +### Changed +- Add common Python files to gitignore (https://github.com/ami-iit/bipedal-locomotion-framework/pull/338) +- General improvements of `blf-calibration-delta-updater` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/361) +- Add the possibility to control a subset of coordinates in `IK::SE3Task` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/356) +- Add the possibility to control a subset of coordinates in `IK::CoMTask` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/357) +- Reduce the duplicate code in IK and TSID (https://github.com/ami-iit/bipedal-locomotion-framework/pull/364) +- `QPFixedBaseTSID` now inherits from `QPTSID` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/366) +- Enable the Current control in `RobotInterface` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/375) +- Add the possibility to disable and enable the PD controllers in `IK::SE3Task` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/373). +- Add the possibility to use manif objects in the ForwardEuler integrator (https://github.com/ami-iit/bipedal-locomotion-framework/pull/379). + +### Fix +- Fixed the crashing of `YarpSensorBridge` while trying to access unconfigured control board sensors data by adding some checks (https://github.com/ami-iit/bipedal-locomotion-framework/pull/378) +- Fixed the compilation of Python bindings (enabled by the `FRAMEWORK_COMPILE_PYTHON_BINDINGS` CMake option) when compiling with Visual Studio (https://github.com/ami-iit/bipedal-locomotion-framework/pull/380). +- Fixed the `TOML` and `YARP` implementation of the parameters handler when a `std::vector` is passed to the `setParameter()` method (https://github.com/ami-iit/bipedal-locomotion-framework/pull/390). + +## [0.2.0] - 2021-06-15 +### Added +- Implement IRobotControl python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/200) +- Implement ISensorBridge python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/203) +- Implement `LeggedOdometry` class as a part of `FloatingBaseEstimators` library and handle arbitrary contacts in `FloatingBaseEstimator`. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/151) +- Implement the possibility to set a desired reference trajectory in the TimeVaryingDCMPlanner. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/208) +- Implement SchmittTriggerDetector python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/213) +- Implement ModelComputationsHelper for quick construction of KinDynComputations object using parameters handler (https://github.com/ami-iit/bipedal-locomotion-framework/pull/216) +- Implement FloatingBaseEstimator and LeggedOdometry python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/218) +- Add spdlog as mandatory dependency of the project (https://github.com/ami-iit/bipedal-locomotion-framework/pull/225) +- Implement `ICameraBridge` and `IPointCloudBridge` interface classes as a part of `PerceptionInterface` library. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/165) +- Implement `RealSense` driver class as a part of `PerceptionCapture` library. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/165) +- Implement `realsense-test` utility application. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/165) +- Implement the inverse kinematics component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/229) +- Implement LinearizedFrictionCone class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/244) +- Added a check on whether the installed public headers have the correct folder structure (https://github.com/ami-iit/bipedal-locomotion-framework/pull/247) +- Implement python bindings for VariablesHandler class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/234) +- Implement `PerceptionFeatures` library and implement `ArucoDetector`. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/159) +- Implement FixedBaseDynamics class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/242) +- Implemented Sink and Source classes (https://github.com/ami-iit/bipedal-locomotion-framework/pull/267) +- Implement the IClock, StdClock and YarpClock classes (https://github.com/ami-iit/bipedal-locomotion-framework/pull/269) +- Implement `YarpCameraBridge` class for Yarp implementation of ICameraBridge (https://github.com/ami-iit/bipedal-locomotion-framework/pull/237) +- Implement `PointCloudProcessor` class and modify `realsense-test` to test point clouds handling with Realsense. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/236) +- Implement `AdvanceableRunner` and `SharedResource` classes in System component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/272) +- Implement `handleQuitSignals()` function in System component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/277) +- Implement TaskSpaceInverseDynamics interface (https://github.com/ami-iit/bipedal-locomotion-framework/pull/279) +- Implement `Wrench` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/279) +- Implement `SO3Task` in `TSID` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/281) +- Implement clone method in ParametersHandler classes (https://github.com/ami-iit/bipedal-locomotion-framework/pull/288) +- Implement `VariablesHandler::clear()` and `VariablesHandler::initialize()` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/291) +- Implement the possibility to set the default contact in the `ContactList` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/297) +- Implement `FixedFootDetector` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/284) +- Implement QPFixedBaseTSID class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/251) +- Implement `YarpImplementation::setFromFile()` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/307) +- Implement `CoMTask` in TSID (https://github.com/ami-iit/bipedal-locomotion-framework/pull/304) +- Implement `YarpParametersHandler` bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/309) +- Implement `contactListMapFromJson()` and `contactListMapToJson()` methods and python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/issues/316) +- Implement a matioCpp-based strain2 sensors' FT-IMU logger example device (https://github.com/ami-iit/bipedal-locomotion-framework/pull/326) +- Implement `TomlImplementation` in `ParametersHandler` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/328) +- Implement blf_calibration_delta_updater.py application (https://github.com/ami-iit/bipedal-locomotion-framework/pull/332) + +### Changed +- Move all the Contacts related classes in Contacts component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/204) +- Move all the ContactDetectors related classes in Contacts component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/209) +- The DCMPlanner and TimeVaryingDCMPlanner initialize functions take as input an std::weak_ptr. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/208) +- Use `Math::StandardAccelerationOfGravitation` instead of hardcoding 9.81. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/211) +- Convert iDynTree types in FloatingBaseEstimators component to Eigen/manif types (https://github.com/ami-iit/bipedal-locomotion-framework/pull/215) +- Use std::optional instead of raw pointer in ISensorBridge. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/226) +- Use `System::LinearTask` in TSID component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/240) +- Restructure python bindings in submodules (https://github.com/ami-iit/bipedal-locomotion-framework/pull/238) +- Integrators and DynamicalSystems are now in the `ContinuousDynamicalSystem` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/242) +- Add Input template class to `System::Advanceable` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/267) +- Add support for landmarks and kinematics-free estimation in `FloatingBaseEstimators`. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/254) +- If FRAMEWORK_DETECT_ACTIVE_PYTHON_SITEPACKAGES is OFF, for Python bindings use installation directory provided by sysconfig Python module. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/274) +- Reduce memory allocation in `YarpSensorBridge` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/278) +- Use `TextLogging` in `VariablesHandler` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/291) +- Fix `YarpImplementation::setParameterPrivate()` when a boolean or a vector of boolean is passed (https://github.com/ami-iit/bipedal-locomotion-framework/pull/311) +- Add `foot_take_off_acceleration` and `foot_take_off_velocity` parameters in the `SwingFootPlanner` class (https://github.com/ami-iit/bipedal-locomotion-framework/issues/323) +- Change the parameters handler verbosity (https://github.com/ami-iit/bipedal-locomotion-framework/pull/330) +- Restore backward compatibility of SwingFootPlanner parameters (https://github.com/ami-iit/bipedal-locomotion-framework/pull/334) +- Bump manif version to 0.0.4 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/339) + +### Fixed +- Fix missing implementation of `YarpSensorBridge::getFailedSensorReads()`. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/202) +- Fixed `mas-imu-test` configuration files after FW fix. +- Fixed the implementation ``YarpSensorBridge::attachAllSixAxisForceTorqueSensors()`. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/231) +- Avoid the "Generating the Urdf Model from" message to appear when doing ccmake. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/243) +- Fixed the installation path of public headers related to perception libraries. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/245) +- Fixed InstallBasicPackageFiles to avoid the same problem of https://github.com/ami-iit/matio-cpp/pull/41 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/253) +- Call `positionInterface->setRefSpeeds()` only once when a position reference is set in `YarpRobotControl` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/271) +- Fix initialization of reference frame for world in `LeggedOdometry` class. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/289) +- `LeggedOdometry::Impl::updateInternalContactStates()` is now called even if the legged odometry is not initialize. This was required to have a meaningful base estimation the first time `LeggedOdometry::changeFixedFrame()` is called. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/292) +- Avoid to use the default copy-constructor and copy-assignment operator in `ContactPhaseList` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/295) +- Fix `toString()` method of `VariablesHandler` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/302) +- Fix in `YarpUtilities::getVectorFromSearchable` when a vector of boolean is passed as input (https://github.com/ami-iit/bipedal-locomotion-framework/pull/313) +- Various fixes for the yarp devices (https://github.com/ami-iit/bipedal-locomotion-framework/pull/337) + +## [0.1.1] - 2021-05-08 +### Fix +- Fix the documentation in `TemplateHelpers.h` + +## [0.1.0] - 2021-02-22 +### Added +- The `CHANGELOG.md` file +- The `cmake/BipedalLocomotionControllersFindDepencies.cmake` file +- The `cmake/AddInstallRPATHSupport.cmake` file +- The `cmake/AddUninstallTarget.cmake` file +- The `cmake/FindEigen3.cmake` file +- The `cmake/InstallBasicPackageFiles.cmake` file +- Implement the first version of the `BipedalLocomotionControllers` interface +- Implement the first version of the `YarpUtilities` library +- Implement `ParametersHandler` library (https://github.com/ami-iit/bipedal-locomotion-controllers/pull/13) +- Implement `GenericContainer::Vector` (https://github.com/ami-iit/bipedal-locomotion-controllers/pull/29) +- Implement `Estimators` library (https://github.com/ami-iit/bipedal-locomotion-controllers/pull/23) +- Implement `Contact` library. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/43 and https://github.com/ami-iit/bipedal-locomotion-framework/pull/45) +- Implement the first version of the `TimeVaryingDCMPlanner` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/61) +- Implement the Quintic Spline class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/83) +- Implement the `ConvexHullHelper` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/51) +- Implement the `DynamicalSystem` and `Integrator` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/46) +- Implement the `IRobotControl` interface and the YARP specialization (https://github.com/ami-iit/bipedal-locomotion-framework/pull/97, https://github.com/ami-iit/bipedal-locomotion-framework/pull/192) +- Add `SensorBridge` interface (https://github.com/ami-iit/bipedal-locomotion-framework/pull/87) +- Add the `YarpSensorBridge` Implementation (https://github.com/ami-iit/bipedal-locomotion-framework/pull/106) +- Added `CommonConversions`, `ManifConversions`, and `matioCppConversions` libraries to handle type conversions. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/138 and https://github.com/ami-iit/bipedal-locomotion-framework/pull/143) +- Implement the `JointPositionTracking` application. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/136) +- Initial implementation of Python bindings using pybind11 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/134) +- Implement `FloatingBaseEstimatorDevice` YARP device for wrapping floating base estimation algorithms. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/130) +- Implement Continuous algebraic Riccati equation function (https://github.com/ami-iit/bipedal-locomotion-framework/pull/157) +- Implement YARP based `ROSPublisher` in the `YarpUtilities` library. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/156) +- Implement example YARP device `ROSPublisherTestDevice` for understanding the usage of `ROSPublisher`. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/160) +- Implement `TSID` library. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/167, https://github.com/ami-iit/bipedal-locomotion-framework/pull/170, https://github.com/ami-iit/bipedal-locomotion-framework/pull/178) +- Implement the `JointTrajectoryPlayer` application. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/169)29ed234a1c +- Implement `ContactDetectors` library. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/142) +- Added `mas-imu-test` application to check the output of MAS IMUs (https://github.com/ami-iit/bipedal-locomotion-framework/pull/62) +- Implement motor currents reading in `YarpSensorBridge`. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/187) + +[unreleased]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.12.0...master +[0.12.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.11.1...v0.12.0 +[0.11.1]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.11.0...v0.11.1 +[0.11.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.10.0...v0.11.0 +[0.10.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.9.0...v0.10.0 +[0.9.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.8.0...v0.9.0 +[0.8.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.7.0...v0.8.0 +[0.7.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.6.0...v0.7.0 +[0.6.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.5.0...v0.6.0 +[0.5.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.4.0...v0.5.0 +[0.4.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.3.0...v0.4.0 +[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 diff --git a/src/Estimators/CMakeLists.txt b/src/Estimators/CMakeLists.txt index 65df49b47e..2e0ccff031 100644 --- a/src/Estimators/CMakeLists.txt +++ b/src/Estimators/CMakeLists.txt @@ -28,11 +28,13 @@ if(FRAMEWORK_COMPILE_RobotDynamicsEstimator) add_bipedal_locomotion_library( NAME RobotDynamicsEstimator SOURCES src/SubModel.cpp src/SubModelKinDynWrapper.cpp src/Dynamics.cpp src/ZeroVelocityDynamics.cpp src/FrictionTorqueStateDynamics.cpp - src/JointVelocityStateDynamics.cpp src/UkfState.cpp src/SubModelDynamics.cpp + src/JointVelocityStateDynamics.cpp src/UkfState.cpp src/SubModelDynamics.cpp src/MotorCurrentMeasurementDynamics.cpp + src/UkfMeasurement.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}/ZeroVelocityDynamics.h - ${H_PREFIX}/FrictionTorqueStateDynamics.h ${H_PREFIX}/JointVelocityStateDynamics.h - ${H_PREFIX}/UkfState.h ${H_PREFIX}/SubModelDynamics.h + ${H_PREFIX}/FrictionTorqueStateDynamics.h ${H_PREFIX}/JointVelocityStateDynamics.h ${H_PREFIX}/AccelerometerMeasurementDynamics.h + ${H_PREFIX}/UkfState.h ${H_PREFIX}/SubModelDynamics.h ${H_PREFIX}/MotorCurrentMeasurementDynamics.h ${H_PREFIX}/UkfMeasurement.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 MANIF::manif BayesFilters::BayesFilters PRIVATE_LINK_LIBRARIES BipedalLocomotion::TextLogging BipedalLocomotion::ManifConversions diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h new file mode 100644 index 0000000000..9652528163 --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h @@ -0,0 +1,135 @@ +/** + * @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 +#include +#include + +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. */ + bool m_isSubModelListSet{false}; /**< Boolean flag saying if the sub-model list has been set. */ + double m_dT; /**< Sampling time. */ + int m_nrOfSubDynamics; /**< Number of sub-dynamics which corresponds to the number of sub-models. */ + std::vector> m_subDynamics; /**< Vector of SubModelInversDynamics objects. */ + Eigen::VectorXd m_jointVelocityFullModel; /**< Vector of joint velocities. */ + Eigen::VectorXd m_motorTorqueFullModel; /**< Motor torque vector of full-model. */ + Eigen::VectorXd m_frictionTorqueFullModel; /**< Friction torque vector of full-model. */ + std::vector m_subModelJointAcc; /**< Updated joint acceleration of each sub-model. */ + Eigen::VectorXd m_jointAccelerationFullModel; /**< Vector of joint accelerations. */ + Eigen::Vector3d m_gravity; /**< Gravitational acceleration. */ + std::vector m_subModelsWithAcc; /**< List of indeces saying which sub-model in the m_subDynamics list containa the accelerometer. */ + +protected: + Eigen::VectorXd m_covSingleVar; + manif::SE3d::Tangent m_subModelBaseAcc; + Eigen::VectorXd m_JdotNu; + Eigen::VectorXd m_JvdotBase; + Eigen::VectorXd m_Jsdotdot; + Eigen::Vector3d m_accRg; + +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 | + * | `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 | + * | `dT` | `double` | Sampling time. | Yes | + * @return True in case of success, false otherwise. + */ + bool initialize(std::weak_ptr 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 kinDynWrapper pointer to a SubModelKinDynWrapper object. + * @return True in case of success, false otherwise. + */ + bool setSubModels(const std::vector& subModelList, const std::vector>& 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. + * @param ukfState reference to the ukf state. + */ + void setState(const Eigen::Ref ukfState) override; + + /** + * Set a `UKFInput` object. + * @param ukfInput reference to the UKFInput struct. + */ + void setInput(const UKFInput & ukfInput) override; + +}; // class AccelerometerMeasurementDynamics + +BLF_REGISTER_DYNAMICS(AccelerometerMeasurementDynamics, ::BipedalLocomotion::Estimators::RobotDynamicsEstimator::Dynamics); + +} // namespace RobotDynamicsEstimator +} // namespace Estimators +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_ACCELEROMETER_MEASUREMENT_DYNAMICS_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h index 2aa51777da..98dda46ee1 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h @@ -103,7 +103,7 @@ class Dynamics std::vector m_elements = {}; /**< Elements composing the variable vector. */ std::string m_dynamicModel; bool m_isInitialized{false}; /**< True if the dynamics has been initialized. */ - System::VariablesHandler m_stateVariableHandler; /**< Variable handler describing the variables and the sizes in the ukf state/measurement vector. */ + System::VariablesHandler m_stateVariableHandler; /**< Variable handler describing the variables and the sizes in the ukf state vector. */ bool m_isStateVariableHandlerSet{false}; /**< True if setVariableHandler is called. */ UKFInput m_ukfInput; diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/GyroscopeMeasurementDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/GyroscopeMeasurementDynamics.h new file mode 100644 index 0000000000..82e384a856 --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/GyroscopeMeasurementDynamics.h @@ -0,0 +1,126 @@ +/** + * @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 +#include +#include + +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. */ + double m_dT; /**< Sampling time. */ + int m_nrOfSubDynamics; /**< Number of sub-dynamics which corresponds to the number of sub-models. */ + std::vector> m_subModelKinDynList; /**< Vector of SubModelKinDynWrapper objects. */ + std::vector m_subModelList; /**< Vector of SubModel objects. */ + std::vector m_subModelJointVel; /**< Updated joint velocities of each sub-model. */ + std::vector 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. */ + +protected: + Eigen::VectorXd m_covSingleVar; + manif::SE3d::Tangent m_subModelBaseVel; + Eigen::VectorXd m_JvBase; + Eigen::VectorXd m_Jsdot; + +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 | + * | `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 | + * | `dT` | `double` | Sampling time. | Yes | + * @return True in case of success, false otherwise. + */ + bool initialize(std::weak_ptr 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 kinDynWrapper pointer to a SubModelKinDynWrapper object. + * @return True in case of success, false otherwise. + */ + bool setSubModels(const std::vector& subModelList, const std::vector>& 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. + * @param ukfState reference to the ukf state. + */ + void setState(const Eigen::Ref ukfState) override; + + /** + * Set a `UKFInput` object. + * @param ukfInput reference to the UKFInput struct. + */ + void setInput(const UKFInput & ukfInput) override; + +}; // class GyroscopeMeasurementDynamics + +BLF_REGISTER_DYNAMICS(GyroscopeMeasurementDynamics, ::BipedalLocomotion::Estimators::RobotDynamicsEstimator::Dynamics); + +} // namespace RobotDynamicsEstimator +} // namespace Estimators +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_ACCELEROMETER_MEASUREMENT_DYNAMICS_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/MotorCurrentMeasurementDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/MotorCurrentMeasurementDynamics.h new file mode 100644 index 0000000000..74437f3b9c --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/MotorCurrentMeasurementDynamics.h @@ -0,0 +1,117 @@ +/** + * @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 + +// Eigen +#include + +#include + +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 +{ +protected: + 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. */ + +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 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 kinDynWrapper pointer to a SubModelKinDynWrapper object. + * @return True in case of success, false otherwise. + */ + bool setSubModels(const std::vector& subModelList, const std::vector>& 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. + * @param ukfState reference to the ukf state. + * @return true if the current state has been updated correctly. + */ + void setState(const Eigen::Ref ukfState) override; + + /** + * Set a `UKFInput` object. + * @param ukfInput reference to the UKFInput struct. + */ + void setInput(const UKFInput & ukfInput) override; + +}; + +BLF_REGISTER_DYNAMICS(MotorCurrentMeasurementDynamics, ::BipedalLocomotion::Estimators::RobotDynamicsEstimator::Dynamics); + +} // namespace RobotDynamicsEstimator +} // namespace Estimators +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_MOTOR_CURRENT_MEASUREMENT_DYNAMICS_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h index ddae85d386..0c4bb0c5e2 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h @@ -147,24 +147,70 @@ class SubModel /** * @brief Access an element of the force/torque sensor list. + * @param index is the index of the force/torque sensor in the submodel * @return FT object associated with the specified index. */ const FT& getFTSensor(const int index) const; + /** + * @brief Access an element of the force/torque sensor list. + * @param is the name of the force/torque sensor. + * @return FT object associated with the specified name. + */ + const FT& getFTSensor(const std::string name) const; + + /** + * @brief hasFTSensor check if the force/torque sensor is part of the sub-model + * @param name is the name of the ft sensor + * @return true if the sensor is found, false otherwise + */ + bool hasFTSensor(const std::string name) const; + /** * @brief Access an element of the accelerometer list. + * @param index is the index of the accelerometer in the submodel * @return a Sensor object corresponding to the accelerometer associated with the specified index. */ const Sensor& getAccelerometer(const int index) const; + /** + * @brief Access an element of the accelerometer list. + * @param is the name of the accelerometer. + * @return a Sensor object corresponding to the accelerometer associated with the specified name. + */ + const Sensor& getAccelerometer(const std::string name) const; + + /** + * @brief hasAccelerometer check if the accelerometer is part of the sub-model + * @param name is the name of the accelerometer + * @return true if the sensor is found, false otherwise + */ + bool hasAccelerometer(const std::string name) const; + /** * @brief Access an element of the gyroscope list. + * @param index is the index of the gyroscope in the submodel * @return a Sensor object corresponding to the gyroscope associated with the specified index. */ const Sensor& getGyroscope(const int index) const; + /** + * @brief Access an element of the gyroscope list. + * @param is the name of the gyroscoper. + * @return a Sensor object corresponding to the gyroscope associated with the specified name. + */ + const Sensor& getGyroscope(const std::string name) const; + + /** + * @brief hasAccelerometer check if the gyroscope is part of the sub-model + * @param name is the name of the gyroscope + * @return true if the sensor is found, false otherwise + */ + bool hasGyroscope(const std::string name) const; + /** * @brief access an element of the contact frame list. + * @param index is the index of the external contact in the submodel. * @return a string corresponding to the external contact frame associated with the specified index. */ const std::string& getExternalContact(const int index) const; diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfMeasurement.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfMeasurement.h new file mode 100644 index 0000000000..094cf653a2 --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfMeasurement.h @@ -0,0 +1,240 @@ +/** + * @file UkfMeasurement.h + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_UKF_MEASUREMENT_H +#define BIPEDAL_LOCOMOTION_ESTIMATORS_UKF_MEASUREMENT_H + +#include +#include +#include + +// BayesFilters +#include + +// BLF +#include +#include +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace Estimators +{ +namespace RobotDynamicsEstimator +{ + +/** + * UkfMeasurement is a concrete class that represents the Measurement of the estimator. + * The user should build the dynamic model of the measurement, setting a variable handler + * describing the variables composing the measurement vector, the list of the dynamic models + * associated to each variable, and the matrix of covariances associated to the variable in the + * measurement vector. The user should set also a ukf input provider + * which provides the inputs needed to update the dynamics. + */ + +class UkfMeasurement : public bfl::AdditiveMeasurementModel +{ + /** + * Private implementation + */ + struct Impl; + std::unique_ptr m_pimpl; + +public: + /** + * Constructor. + */ + UkfMeasurement(); + + /** + * Destructor. + */ + virtual ~UkfMeasurement(); + + /** + * Build the ukf measurement model + * @param kinDyn a pointer to an iDynTree::KinDynComputations object that will be shared among + * all the dynamics. + * @param subModelList a vector of SubModel objects. + * @param kinDynWrapperList a vector of pointers to SubModelKinDynWrapper objects + * @param handler pointer to the IParametersHandler interface. + * @param stateVariableHandler a variable handler describing the variables in the state vector of the ukf. + * @note the following parameters are required by the class + * | Parameter Name | Type | Description | Mandatory | + * |:------------------------------:|:---------------:|:----------------------------------------------------------------------------------------------:|:---------:| + * | `sampling_time` | `double` | Sampling time. | Yes | + * | `dynamics_list` |`vector` | List of dynamics composing the measurement model. | Yes | + * For **each** dynamics listed in the parameter `dynamics_list` the user must specified all the parameters + * required by the dynamics itself but `dT` since is already specified in the parent group. + * Moreover the following parameters are required for each dynamics. + * | Group | Parameter Name | Type | Description | Mandatory | + * |:-------------:|:------------------------------:|:--------------------:|:----------------------------------------------------------------------------------------------:|:---------:| + * |`DYNAMICS_NAME`| `name` | `string` | String representing the name of the dynamics. | Yes | + * |`DYNAMICS_NAME`| `elements` |`std::vector` | Vector of strings representing the elements composing the specific dynamics. | No | + * |`DYNAMICS_NAME`| `covariance` |`std::vector` | Vector of double containing the covariance associated to each element. | Yes | + * |`DYNAMICS_NAME`| `dynamic_model` | `string` | String representing the type of dynamics. The string should match the name of the C++ class. | Yes | + * |`DYNAMICS_NAME`| `use_bias` | `boolean` | Boolean saying if an additive bias must be used in the dynamic model. | No | + * |`DYNAMICS_NAME`| `use_bias` | `boolean` | Boolean saying if an additive bias must be used in the dynamic model. | No | + * `DYNAMICS_NAME` is a placeholder for the name of the dynamics contained in the `dynamics_list` list. + * `name` can contain only the following values ("ds", "i_m", "*_ft_sensor", "*_ft_acc", "*_ft_gyro"). + * @note The following `ini` file presents an example of the configuration that can be used to + * build the UkfMeasurement. + * + * UkfMeasurement.ini + * + * dynamics_list ("JOINT_VELOCITY", "MOTOR_CURRENT", "RIGHT_LEG_FT", "RIGHT_FOOT_REAR_GYRO") + * + * [JOINT_VELOCITY] + * name "ds" + * elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") + * covariance (1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3) + * dynamic_model "JointVelocityDynamics" + * + * [FRICTION_TORQUE] + * name "tau_F" + * elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") + * covariance (1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-1) + * dynamic_model "FrictionTorqueDynamics" + * friction_k0 (9.106, 5.03, 4.93, 12.88, 14.34, 1.12) + * friction_k1 (200.0, 6.9, 200.0, 59.87, 200.0, 200.0) + * friction_k2 (1.767, 5.64, 0.27, 2.0, 3.0, 0.0) + * + * [RIGHT_LEG_FT] + * name "r_leg_ft_sensor" + * elements ("fx", "fy", "fz", "mx", "my", "mz") + * covariance (1e-3, 1e-3, 1e-3, 1e-4, 1e-4, 1e-4) + * dynamic_model "ZeroVelocityDynamics" + * + * [RIGHT_FOOT_REAR_GYRO_BIAS] + * name "r_foot_rear_ft_gyro_bias" + * elements ("x", "y", "z") + * covariance (8.2e-8, 1e-2, 9.3e-3) + * dynamic_model "ZeroVelocityDynamics" + * + * ~~~~~ + * @return a std::unique_ptr to the UkfMeasurement. + * In case of issues, an empty BipedalLocomotion::System::VariablesHandler + * and an invalid pointer will be returned. + */ + static std::unique_ptr build(std::weak_ptr handler, + System::VariablesHandler& stateVariableHandler, + std::shared_ptr kinDynFullModel, + const std::vector& subModelList, + const std::vector>& kinDynWrapperList); + + /** + * Initialize the ukf measurement model. + * @param handler pointer to the IParametersHandler interface. + * @note the following parameters are required by the class + * | Parameter Name | Type | Description | Mandatory | + * |:------------------------------:|:---------------:|:----------------------------------------------------------------------------------------------:|:---------:| + * | `sampling_time` | `double` | Sampling time. | Yes | + * @return True in case of success, false otherwise. + */ + bool initialize(std::weak_ptr handler); + + /** + * Finalize the UkfMeasurement. + * @param handler variable handler. + * @note You should call this method after you initialize the UkfMeasurement. + * @return true in case of success, false otherwise. + */ + bool finalize(const System::VariablesHandler& handler); + + /** + * @brief setUkfInputProvider set the provider for the ukf input. + * @param ukfInputProvider a pointer to a structure containing the joint positions and the robot base pose, velocity and acceleration. + */ + void setUkfInputProvider(std::shared_ptr ukfInputProvider); + + /** + * @brief getMeasurementVariableHandler access the `System::VariablesHandler` instance created during the initialization phase. + * @return the measurement variable handler containing all the measurement variables and their sizes and offsets. + */ + System::VariablesHandler getMeasurementVariableHandler(); + + /** + * @brief predictedMeasure predict the new measurement depending on the state computed by the predict step. + * @param cur_states is the state computed by the prediction phase. + * @return a std::pair where the bool value says if the measurement + * prediciton is done correctly and the bfl::Data is the predicted measure. + */ + std::pair predictedMeasure(const Eigen::Ref& cur_states) const override; + + /** + * @brief getNoiseCovarianceMatrix access the `Eigen::MatrixXd` representing the process covariance matrix. + * @return a boolean value and the measurement noise covariance matrix. + */ + std::pair getNoiseCovarianceMatrix() const override; + + /** + * @brief setProperty is not implemented. + * @param property is a string. + * @return false as it is not implemented. + */ + bool setProperty(const std::string& property) override { return false; }; + + /** + * @brief getMeasurementDescription access the `bfl::VectorDescription`. + * @return the measurement vector description. + */ + bfl::VectorDescription getMeasurementDescription() const override; + + /** + * @brief getInputDescription access the `bfl::VectorDescription`. + * @return the input vector description. + */ + bfl::VectorDescription getInputDescription() const override; + + /** + * @brief getMeasurementSize access the length of the measurement vector + * @return the length of measurement vector + */ + std::size_t getMeasurementSize(); + + /** + * Set a `System::VariableHandler` describing the variables composing the state. + * @param stateVariableHandler is the variable handler + */ + void setStateVariableHandler(System::VariablesHandler stateVariableHandler) const; + + /** + * @brief innovation computes the innovation step of the ukf update as the difference between the predicted_measurement + * and the measurement. + * @param predicted_measurements is a `blf::Data` reference representing the measurement predicted in the update step. + * @param measurements is a `blf::Data` reference representing the measurements coming from the user. + * @return a `std::pair` where the boolean value is always true and the `bfl::Data` is the innovation term. + */ + std::pair innovation(const bfl::Data& predicted_measurements, const bfl::Data& measurements) const override; + + /** + * @brief measure get the updated measurement. + * @param data is a `const blf::Data` reference and is optional parameter. + * @return a pair of a boolean value which is always true and the measurements. + */ + std::pair measure(const bfl::Data& data = bfl::Data()) const override; + + /** + * @brief freeze update the measurement using data from sensors. + * @param data is a generic object representing data coming from sensors. + * @return true. + * @note data in this case must be a `std::map` where + * the first element represents the name of each measurement dynamics + * and the second element is the vector containing the measurement + * of the sensor associated to that variable. + */ + bool freeze(const bfl::Data& data = bfl::Data()) override; + +}; // classUkfMeasurement + +} // namespace RobotDynamicsEstimator +} // namespace Estimators +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_UKF_MEASUREMENT_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h index 553a2ff0cd..288d25b9f7 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h @@ -56,7 +56,7 @@ class ZeroVelocityDynamics : public Dynamics * | `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 | - * | `bias` |`boolean` | Boolean saying if the dynamics depends on a bias. False if not specified. | 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; diff --git a/src/Estimators/src/AccelerometerMeasurementDynamics.cpp b/src/Estimators/src/AccelerometerMeasurementDynamics.cpp new file mode 100644 index 0000000000..c29ade2a98 --- /dev/null +++ b/src/Estimators/src/AccelerometerMeasurementDynamics.cpp @@ -0,0 +1,338 @@ +/** + * @file AccelerometerMeasurementDynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include +#include + +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +RDE::AccelerometerMeasurementDynamics::AccelerometerMeasurementDynamics() = default; + +RDE::AccelerometerMeasurementDynamics::~AccelerometerMeasurementDynamics() = default; + +bool RDE::AccelerometerMeasurementDynamics::initialize(std::weak_ptr paramHandler) +{ + constexpr auto errorPrefix = "[AccelerometerMeasurementDynamics::initialize]"; + + auto ptr = paramHandler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is not valid.", errorPrefix); + return false; + } + + // Set the state dynamics name + if (!ptr->getParameter("name", m_name)) + { + log()->error("{} Error while retrieving the name variable.", errorPrefix); + return false; + } + + // Set the state process covariance + if (!ptr->getParameter("covariance", m_covSingleVar)) + { + log()->error("{} Error while retrieving the covariance variable.", errorPrefix); + return false; + } + + // Set the dynamic model type + if (!ptr->getParameter("dynamic_model", m_dynamicModel)) + { + log()->error("{} Error while retrieving the dynamic_model variable.", errorPrefix); + return false; + } + + // Set the list of elements if it exists + if (!ptr->getParameter("elements", m_elements)) + { + log()->info("{} Variable elements not found.", errorPrefix); + m_elements = {}; + } + + if (!ptr->getParameter("sampling_time", m_dT)) + { + log()->error("{} Error while retrieving the sampling_time variable.", errorPrefix); + return false; + } + + // Set the bias related variables if use_bias is true + if (!ptr->getParameter("use_bias", m_useBias)) + { + log()->info("{} Variable use_bias not found. Set to false by default.", errorPrefix); + } + else + { + m_useBias = true; + m_biasVariableName = m_name + "_bias"; + } + + m_gravity.setZero(); + m_gravity[2] = - BipedalLocomotion::Math::StandardAccelerationOfGravitation; + + m_JdotNu.resize(6); + m_JvdotBase.resize(6); + m_Jsdotdot.resize(6); + + m_description = "Accelerometer measurement dynamics"; + + m_isInitialized = true; + + return true; +} + +bool RDE::AccelerometerMeasurementDynamics::finalize(const System::VariablesHandler &stateVariableHandler) +{ + constexpr auto errorPrefix = "[AccelerometerMeasurementDynamics::finalize]"; + + if (!m_isInitialized) + { + log()->error("{} Please initialize the dynamics before calling finalize.", errorPrefix); + return false; + } + + if (stateVariableHandler.getNumberOfVariables() == 0) + { + log()->error("{} The state variable handler is empty.", errorPrefix); + return false; + } + + m_stateVariableHandler = stateVariableHandler; + + if (!checkStateVariableHandler()) + { + log()->error("{} The state variable handler is not valid.", errorPrefix); + return false; + } + + // Search and save all the submodels containing the sensor + for (int submodelIndex = 0; submodelIndex < m_nrOfSubDynamics; submodelIndex++) + { + if (m_subDynamics[submodelIndex]->subModel.hasAccelerometer(m_name)) + { + m_subModelsWithAcc.push_back(submodelIndex); + } + } + + m_covariances.resize(m_covSingleVar.size() * m_subModelsWithAcc.size()); + for (int index = 0; index < m_subModelsWithAcc.size(); index++) + { + m_covariances.segment(index*m_covSingleVar.size(), m_covSingleVar.size()) = m_covSingleVar; + } + + m_size = m_covariances.size(); + + m_motorTorqueFullModel.resize(m_stateVariableHandler.getVariable("tau_m").size); + m_motorTorqueFullModel.setZero(); + + m_frictionTorqueFullModel.resize(m_stateVariableHandler.getVariable("tau_F").size); + m_frictionTorqueFullModel.setZero(); + + m_jointAccelerationFullModel.resize(m_stateVariableHandler.getVariable("ds").size); + m_jointAccelerationFullModel.setZero(); + + m_jointVelocityFullModel.resize(m_stateVariableHandler.getVariable("ds").size); + m_jointVelocityFullModel.setZero(); + + m_subModelJointAcc.resize(m_nrOfSubDynamics); + + for (int idx = 0; idx < m_nrOfSubDynamics; idx++) + { + m_subModelJointAcc[idx].resize(m_subDynamics[idx]->subModel.getJointMapping().size()); + m_subModelJointAcc[idx].setZero(); + } + + m_bias.resize(m_covSingleVar.size()); + m_bias.setZero(); + + m_updatedVariable.resize(m_size); + m_updatedVariable.setZero(); + + return true; +} + +bool RDE::AccelerometerMeasurementDynamics::setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) +{ + constexpr auto errorPrefix = "[AccelerometerMeasurementDynamics::setSubModels]"; + + for (int subModelIdx = 0; subModelIdx < subModelList.size(); subModelIdx++) + { + m_subDynamics.emplace_back(std::make_unique()); + + if(!m_subDynamics[subModelIdx]->setSubModel(subModelList[subModelIdx])) + { + log()->error("{} The submodel at index {} is not valid.", errorPrefix, subModelIdx); + return false; + } + + if(!m_subDynamics[subModelIdx]->setKinDynWrapper(kinDynWrapperList[subModelIdx])) + { + log()->error("{} The submodel kindyn wrapper at index {} is not valid.", errorPrefix, subModelIdx); + return false; + } + } + + m_nrOfSubDynamics = m_subDynamics.size(); + + m_isSubModelListSet = true; + + for (int subModelIdx = 0; subModelIdx < m_nrOfSubDynamics; subModelIdx++) + { + if (!m_subDynamics.at(subModelIdx)->initialize()) + { + log()->error("{} Cannot initialize the accelerometer measurement dynamics of the sub-model {}.", errorPrefix, subModelIdx); + return false; + } + } + + return true; +} + +bool RDE::AccelerometerMeasurementDynamics::checkStateVariableHandler() +{ + constexpr auto errorPrefix = "[AccelerometerMeasurementDynamics::checkStateVariableHandler]"; + + if (!m_isSubModelListSet) + { + log()->error("{} Set the sub-model list before setting the variable handler", errorPrefix); + return false; + } + + if (!m_stateVariableHandler.getVariable("tau_m").isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `tau_m`.", errorPrefix); + return false; + } + + // Check if the variable handler contains the variables used by this dynamics + if (!m_stateVariableHandler.getVariable("tau_F").isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `tau_F`.", errorPrefix); + return false; + } + + if (!m_stateVariableHandler.getVariable("ds").isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `ds`.", errorPrefix); + return false; + } + + if (m_useBias) + { + if (!m_stateVariableHandler.getVariable(m_biasVariableName).isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_biasVariableName); + return false; + } + } + + // check if all the sensors are part of the sub-model + for (int subModelIdx = 0; subModelIdx < m_nrOfSubDynamics; subModelIdx++) + { + for (int ftIdx = 0; ftIdx < m_subDynamics.at(subModelIdx)->subModel.getNrOfFTSensor(); ftIdx++) + { + if (!m_stateVariableHandler.getVariable(m_subDynamics.at(subModelIdx)->subModel.getFTSensor(ftIdx).name).isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_subDynamics.at(subModelIdx)->subModel.getFTSensor(ftIdx).name); + return false; + } + } + + for (int contactIdx = 0; contactIdx < m_subDynamics.at(subModelIdx)->subModel.getNrOfExternalContact(); contactIdx++) + { + if (!m_stateVariableHandler.getVariable(m_subDynamics.at(subModelIdx)->subModel.getExternalContact(contactIdx)).isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_subDynamics.at(subModelIdx)->subModel.getExternalContact(contactIdx)); + return false; + } + } + } + + return true; +} + +bool RDE::AccelerometerMeasurementDynamics::update() +{ + constexpr auto errorPrefix = "[AccelerometerMeasurementDynamics::update]"; + + // compute joint acceleration per each sub-model containing the accelerometer + for (int subDynamicsIdx = 0; subDynamicsIdx < m_subDynamics.size(); subDynamicsIdx++) + { + if (m_subDynamics[subDynamicsIdx]->subModel.getModel().getNrOfDOFs() > 0) + { + if (!m_subDynamics[subDynamicsIdx]->update(m_ukfInput.robotBaseAcceleration, m_jointAccelerationFullModel, m_subModelJointAcc[subDynamicsIdx])) + { + log()->error("{} Error updating the joint velocity dynamics for the sub-model {}.", errorPrefix, subDynamicsIdx); + return false; + } + + for (int jointIdx = 0; jointIdx < m_subDynamics[subDynamicsIdx]->subModel.getJointMapping().size(); jointIdx++) + { + m_jointAccelerationFullModel[m_subDynamics[subDynamicsIdx]->subModel.getJointMapping()[jointIdx]] = m_subModelJointAcc[subDynamicsIdx][jointIdx]; + } + } + } + + // J_dot nu + base_J v_dot_base + joint_J s_dotdot - acc_Rot_world gravity + bias + for (int index = 0; index < m_subModelsWithAcc.size(); index++) + { + m_JdotNu = m_subDynamics[m_subModelsWithAcc[index]]->kinDynWrapper->getAccelerometerBiasAcceleration(m_name); + + if (!m_subDynamics[m_subModelsWithAcc[index]]->kinDynWrapper->getBaseAcceleration(m_ukfInput.robotBaseAcceleration, m_jointAccelerationFullModel, m_subModelBaseAcc)) + { + log()->error("{} Error getting the sub-model base acceleration.", errorPrefix); + return false; + } + + m_JvdotBase = m_subDynamics[m_subModelsWithAcc[index]]->kinDynWrapper->getAccelerometerJacobian(m_name).block(0, 0, 6, 6) * m_subModelBaseAcc.coeffs(); + + m_accRg = m_subDynamics[m_subModelsWithAcc[index]]->kinDynWrapper->getAccelerometerRotation(m_name).rotation() * m_gravity; + + m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) = m_JdotNu.segment(0, 3) + m_JvdotBase.segment(0, 3) - m_accRg + m_bias; + + if (m_subDynamics[m_subModelsWithAcc[index]]->subModel.getJointMapping().size() > 0) + { + m_Jsdotdot = m_subDynamics[m_subModelsWithAcc[index]]->kinDynWrapper->getAccelerometerJacobian(m_name).block(0, 6, 6, m_subModelJointAcc[m_subModelsWithAcc[index]].size()) * + m_subModelJointAcc[m_subModelsWithAcc[index]]; + + m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) += m_Jsdotdot.segment(0, 3); + } + } + + return true; +} + +void RDE::AccelerometerMeasurementDynamics::setState(const Eigen::Ref ukfState) +{ + m_jointVelocityFullModel = ukfState.segment(m_stateVariableHandler.getVariable("ds").offset, + m_stateVariableHandler.getVariable("ds").size); + + m_motorTorqueFullModel = ukfState.segment(m_stateVariableHandler.getVariable("tau_m").offset, + m_stateVariableHandler.getVariable("tau_m").size); + + m_frictionTorqueFullModel = ukfState.segment(m_stateVariableHandler.getVariable("tau_F").offset, + m_stateVariableHandler.getVariable("tau_F").size); + + m_bias = ukfState.segment(m_stateVariableHandler.getVariable(m_biasVariableName).offset, + m_stateVariableHandler.getVariable(m_biasVariableName).size); + + for (int subDynamicsIdx = 0; subDynamicsIdx < m_subDynamics.size(); subDynamicsIdx++) + { + m_subDynamics.at(subDynamicsIdx)->setState(ukfState, + m_jointVelocityFullModel, + m_motorTorqueFullModel, + m_frictionTorqueFullModel, + m_stateVariableHandler); + } +} + +void RDE::AccelerometerMeasurementDynamics::setInput(const UKFInput& ukfInput) +{ + m_ukfInput = ukfInput; +} diff --git a/src/Estimators/src/GyroscopeMeasurementDynamics.cpp b/src/Estimators/src/GyroscopeMeasurementDynamics.cpp new file mode 100644 index 0000000000..acae98f028 --- /dev/null +++ b/src/Estimators/src/GyroscopeMeasurementDynamics.cpp @@ -0,0 +1,238 @@ +/** + * @file GyroscopeMeasurementDynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include +#include + +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +RDE::GyroscopeMeasurementDynamics::GyroscopeMeasurementDynamics() = default; + +RDE::GyroscopeMeasurementDynamics::~GyroscopeMeasurementDynamics() = default; + +bool RDE::GyroscopeMeasurementDynamics::initialize(std::weak_ptr paramHandler) +{ + constexpr auto errorPrefix = "[GyroscopeMeasurementDynamics::initialize]"; + + auto ptr = paramHandler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is not valid.", errorPrefix); + return false; + } + + // Set the state dynamics name + if (!ptr->getParameter("name", m_name)) + { + log()->error("{} Error while retrieving the name variable.", errorPrefix); + return false; + } + + // Set the state process covariance + if (!ptr->getParameter("covariance", m_covSingleVar)) + { + log()->error("{} Error while retrieving the covariance variable.", errorPrefix); + return false; + } + + // Set the dynamic model type + if (!ptr->getParameter("dynamic_model", m_dynamicModel)) + { + log()->error("{} Error while retrieving the dynamic_model variable.", errorPrefix); + return false; + } + + // Set the list of elements if it exists + if (!ptr->getParameter("elements", m_elements)) + { + log()->info("{} Variable elements not found.", errorPrefix); + m_elements = {}; + } + + if (!ptr->getParameter("sampling_time", m_dT)) + { + log()->error("{} Error while retrieving the sampling_time variable.", errorPrefix); + return false; + } + + // Set the bias related variables if use_bias is true + if (!ptr->getParameter("use_bias", m_useBias)) + { + log()->info("{} Variable use_bias not found. Set to false by default.", errorPrefix); + } + else + { + m_useBias = true; + m_biasVariableName = m_name + "_bias"; + } + + m_description = "Gyroscope measurement dynamics"; + + m_isInitialized = true; + + return true; +} + +bool RDE::GyroscopeMeasurementDynamics::finalize(const System::VariablesHandler &stateVariableHandler) +{ + constexpr auto errorPrefix = "[GyroscopeMeasurementDynamics::finalize]"; + + if (!m_isInitialized) + { + log()->error("{} Please initialize the dynamics before calling finalize.", errorPrefix); + return false; + } + + if (stateVariableHandler.getNumberOfVariables() == 0) + { + log()->error("{} The state variable handler is empty.", errorPrefix); + return false; + } + + m_stateVariableHandler = stateVariableHandler; + + if (!checkStateVariableHandler()) + { + log()->error("{} The state variable handler is not valid.", errorPrefix); + return false; + } + + // Search and save all the submodels containing the sensor + for (int submodelIndex = 0; submodelIndex < m_nrOfSubDynamics; submodelIndex++) + { + if (m_subModelList[submodelIndex].hasGyroscope(m_name)) + { + m_subModelWithGyro.push_back(submodelIndex); + } + } + + m_covariances.resize(m_covSingleVar.size() * m_subModelWithGyro.size()); + for (int index = 0; index < m_subModelWithGyro.size(); index++) + { + m_covariances.segment(index*m_covSingleVar.size(), m_covSingleVar.size()) = m_covSingleVar; + } + + m_size = m_covariances.size(); + + m_jointVelocityFullModel.resize(m_stateVariableHandler.getVariable("ds").size); + m_jointVelocityFullModel.setZero(); + + m_subModelJointVel.resize(m_nrOfSubDynamics); + + for (int idx = 0; idx < m_nrOfSubDynamics; idx++) + { + m_subModelJointVel[idx].resize(m_subModelList[idx].getJointMapping().size()); + m_subModelJointVel[idx].setZero(); + } + + m_bias.resize(m_covSingleVar.size()); + m_bias.setZero(); + + m_updatedVariable.resize(m_size); + m_updatedVariable.setZero(); + + return true; +} + +bool RDE::GyroscopeMeasurementDynamics::setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) +{ + constexpr auto errorPrefix = "[GyroscopeMeasurementDynamics::setSubModels]"; + + m_subModelList = subModelList; + m_subModelKinDynList = kinDynWrapperList; + + if (m_subModelList.size() == 0 || m_subModelKinDynList.size() == 0 || m_subModelList.size() != m_subModelKinDynList.size()) + { + log()->error("{} Wrong size of input parameters", errorPrefix); + return false; + } + + m_nrOfSubDynamics = m_subModelList.size(); + + m_isSubModelListSet = true; + + return true; +} + +bool RDE::GyroscopeMeasurementDynamics::checkStateVariableHandler() +{ + constexpr auto errorPrefix = "[GyroscopeMeasurementDynamics::checkStateVariableHandler]"; + + if (!m_isSubModelListSet) + { + log()->error("{} Set the sub-model list before setting the variable handler", errorPrefix); + return false; + } + + if (!m_stateVariableHandler.getVariable("ds").isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `ds`.", errorPrefix); + return false; + } + + if (m_useBias) + { + if (!m_stateVariableHandler.getVariable(m_biasVariableName).isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_biasVariableName); + return false; + } + } + + return true; +} + +bool RDE::GyroscopeMeasurementDynamics::update() +{ + constexpr auto errorPrefix = "[GyroscopeMeasurementDynamics::update]"; + + // base_J v_base + joint_J s_dot + bias + for (int index = 0; index < m_subModelWithGyro.size(); index++) + { + m_subModelBaseVel = m_subModelKinDynList[m_subModelWithGyro[index]]->getBaseVelocity(); + + m_JvBase = m_subModelKinDynList[m_subModelWithGyro[index]]->getGyroscopeJacobian(m_name).block(0, 0, 6, 6) * m_subModelBaseVel.coeffs(); + + m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) = m_JvBase.segment(3, 3) + m_bias; + + if (m_subModelList[m_subModelWithGyro[index]].getJointMapping().size() > 0) + { + m_Jsdot = m_subModelKinDynList[m_subModelWithGyro[index]]->getGyroscopeJacobian(m_name).block(0, 6, 6, m_subModelList[m_subModelWithGyro[index]].getModel().getNrOfDOFs()) * + m_subModelJointVel[m_subModelWithGyro[index]]; + + m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) += m_Jsdot.segment(3, 3); + } + } + + return true; +} + +void RDE::GyroscopeMeasurementDynamics::setState(const Eigen::Ref ukfState) +{ + m_jointVelocityFullModel = ukfState.segment(m_stateVariableHandler.getVariable("ds").offset, + m_stateVariableHandler.getVariable("ds").size); + + for (int smIndex = 0; smIndex < m_subModelList.size(); smIndex++) + { + for (int jntIndex = 0; jntIndex < m_subModelList[smIndex].getModel().getNrOfDOFs(); jntIndex++) + { + m_subModelJointVel[smIndex][jntIndex] = + m_jointVelocityFullModel[m_subModelList[smIndex].getJointMapping()[jntIndex]]; + } + } + + m_bias = ukfState.segment(m_stateVariableHandler.getVariable(m_biasVariableName).offset, + m_stateVariableHandler.getVariable(m_biasVariableName).size); +} + +void RDE::GyroscopeMeasurementDynamics::setInput(const UKFInput& ukfInput) +{ + m_ukfInput = ukfInput; +} diff --git a/src/Estimators/src/MotorCurrentMeasurementDynamics.cpp b/src/Estimators/src/MotorCurrentMeasurementDynamics.cpp new file mode 100644 index 0000000000..2452d9d7ce --- /dev/null +++ b/src/Estimators/src/MotorCurrentMeasurementDynamics.cpp @@ -0,0 +1,149 @@ +/** + * @file MotorCurrentMeasurementDynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include + +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +RDE::MotorCurrentMeasurementDynamics::MotorCurrentMeasurementDynamics() = default; + +RDE::MotorCurrentMeasurementDynamics::~MotorCurrentMeasurementDynamics() = default; + +bool RDE::MotorCurrentMeasurementDynamics::initialize(std::weak_ptr paramHandler) +{ + constexpr auto errorPrefix = "[MotorCurrentMeasurementDynamics::initialize]"; + + auto ptr = paramHandler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is not valid.", errorPrefix); + return false; + } + + // Set the state dynamics name + if (!ptr->getParameter("name", m_name)) + { + log()->error("{} Error while retrieving the name variable.", errorPrefix); + return false; + } + + // Set the state process covariance + if (!ptr->getParameter("covariance", this->m_covariances)) + { + log()->error("{} Error while retrieving the covariance variable.", errorPrefix); + return false; + } + + // Set the dynamic model type + if (!ptr->getParameter("dynamic_model", m_dynamicModel)) + { + log()->error("{} Error while retrieving the dynamic_model variable.", errorPrefix); + return false; + } + + // Set the list of elements if it exists + if (!ptr->getParameter("elements", m_elements)) + { + log()->info("{} Variable elements not found.", errorPrefix); + m_elements = {}; + } + + // Set the torque constants + if (!ptr->getParameter("torque_constant", m_kTau)) + { + log()->error("{} Error while retrieving the torque_constant variable.", errorPrefix); + return false; + } + + // Set the gearbox reduction ratio + if (!ptr->getParameter("gear_ratio", m_gearRatio)) + { + log()->error("{} Error while retrieving the gear_ratio variable.", errorPrefix); + return false; + } + + m_description = "Motor current measurement dynamics"; + + m_isInitialized = true; + + return true; +} + +bool RDE::MotorCurrentMeasurementDynamics::finalize(const System::VariablesHandler &stateVariableHandler) +{ + constexpr auto errorPrefix = "[MotorCurrentMeasurementDynamics::finalize]"; + + if (!m_isInitialized) + { + log()->error("{} Please initialize the dynamics before calling finalize.", errorPrefix); + return false; + } + + if (stateVariableHandler.getNumberOfVariables() == 0) + { + log()->error("{} The variable handler is empty.", errorPrefix); + return false; + } + + m_stateVariableHandler = stateVariableHandler; + + if (!checkStateVariableHandler()) + { + log()->error("{} The variable handler is not valid.", errorPrefix); + return false; + } + + m_size = m_covariances.size(); + + m_updatedVariable.resize(m_size); + m_updatedVariable.setZero(); + + m_motorTorque.resize(m_size); + m_motorTorque.setZero(); + + return true; +} + +bool RDE::MotorCurrentMeasurementDynamics::setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) +{ + return true; +} + +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()) + { + log()->error("{} The variable handler does not contain the expected measurement name {}.", errorPrefix, "tau_m"); + return false; + } + + return true; +} + +bool RDE::MotorCurrentMeasurementDynamics::update() +{ + m_updatedVariable = m_motorTorque.array() / (m_kTau.array() * m_gearRatio.array()); + + return true; +} + +void RDE::MotorCurrentMeasurementDynamics::setState(const Eigen::Ref ukfState) +{ + m_motorTorque = ukfState.segment(m_stateVariableHandler.getVariable("tau_m").offset, + m_stateVariableHandler.getVariable("tau_m").size); +} + +void RDE::MotorCurrentMeasurementDynamics::setInput(const UKFInput& ukfInput) +{ + return; +} diff --git a/src/Estimators/src/SubModel.cpp b/src/Estimators/src/SubModel.cpp index 46147e5490..889db6d367 100644 --- a/src/Estimators/src/SubModel.cpp +++ b/src/Estimators/src/SubModel.cpp @@ -494,16 +494,101 @@ const RDE::FT& RDE::SubModel::getFTSensor(const int index) const return m_ftList.at(index); } +const RDE::FT& RDE::SubModel::getFTSensor(const std::string name) const +{ + for (int ftIndex = 0; ftIndex < m_ftList.size(); ftIndex++) + { + if (m_ftList[ftIndex].name == name) + { + return m_ftList[ftIndex]; + } + } + + static const RDE::FT ft; + + return ft; +} + +bool RDE::SubModel::hasFTSensor(const std::string name) const +{ + for (int ftIndex = 0; ftIndex < m_ftList.size(); ftIndex++) + { + if (m_ftList[ftIndex].name == name) + { + return true; + } + } + + return false; +} + const RDE::Sensor& RDE::SubModel::getAccelerometer(const int index) const { return m_accelerometerList.at(index); } +const RDE::Sensor& RDE::SubModel::getAccelerometer(const std::string name) const +{ + for (int accIndex = 0; accIndex < m_accelerometerList.size(); accIndex++) + { + if (m_accelerometerList[accIndex].name == name) + { + return m_accelerometerList[accIndex]; + } + } + + static const RDE::Sensor acc; + + return acc; +} + +bool RDE::SubModel::hasAccelerometer(const std::string name) const +{ + std::cout << "Name --> " << name << std::endl; + for (int accIndex = 0; accIndex < m_accelerometerList.size(); accIndex++) + { + if (m_accelerometerList[accIndex].name == name) + { + return true; + } + } + + return false; +} + const RDE::Sensor& RDE::SubModel::getGyroscope(const int index) const { return m_gyroscopeList.at(index); } +const RDE::Sensor& RDE::SubModel::getGyroscope(const std::string name) const +{ + for (int gyroIndex = 0; gyroIndex < m_gyroscopeList.size(); gyroIndex++) + { + if (m_gyroscopeList[gyroIndex].name == name) + { + return m_gyroscopeList[gyroIndex]; + } + } + + static const RDE::Sensor gyro; + + return gyro; +} + +bool RDE::SubModel::hasGyroscope(const std::string name) const +{ + for (int gyroIndex = 0; gyroIndex < m_gyroscopeList.size(); gyroIndex++) + { + if (m_gyroscopeList[gyroIndex].name == name) + { + return true; + } + } + + return false; +} + const std::string& RDE::SubModel::getExternalContact(const int index) const { return m_externalContactList.at(index); diff --git a/src/Estimators/src/UkfMeasurement.cpp b/src/Estimators/src/UkfMeasurement.cpp new file mode 100644 index 0000000000..d7c1de9969 --- /dev/null +++ b/src/Estimators/src/UkfMeasurement.cpp @@ -0,0 +1,383 @@ +/** + * @file UkfMeasurement.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include + +#include +#include +#include +#include + +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +struct RDE::UkfMeasurement::Impl +{ + bool isInitialized{false}; + bool isFinalized{false}; + + bfl::VectorDescription measurementDescription_; + bfl::VectorDescription inputDescription_; + + Eigen::Vector3d gravity{0, 0, -Math::StandardAccelerationOfGravitation}; /**< Gravity vector. */ + + Eigen::MatrixXd covarianceR; /**< Covariance matrix. */ + std::size_t measurementSize; /**< Length of the measurement vector. */ + double dT; /**< Sampling time */ + + std::map> dynamicsList; /**< List of the dynamics composing the process model. */ + + System::VariablesHandler measurementVariableHandler; /**< Variable handler describing the measurement vector. */ + System::VariablesHandler stateVariableHandler; /**< Variable handler describing the state vector. */ + + std::shared_ptr kinDynFullModel; /**< KinDynComputation object for the full model. */ + std::vector subModelList; /**< List of SubModel object describing the sub-models composing the full model. */ + std::vector> kinDynWrapperList; /**< List of SubModelKinDynWrapper objects containing kinematic and dynamic information specific of each sub-model. */ + + std::shared_ptr ukfInputProvider; /**< Provider containing the updated robot state. */ + UKFInput ukfInput; /**< Struct containing the inputs for the ukf populated by the ukfInputProvider. */ + + Eigen::VectorXd jointVelocityState; /**< Joint velocity compute by the ukf. */ + + Eigen::VectorXd currentState; /**< State estimated in the previous step. */ + Eigen::VectorXd predictedMeasurement; /**< Vector containing the updated measurement. */ + + Eigen::VectorXd measurement; /**< Measurements coming from the sensors. */ + + std::map measurementMap; /**< Measurement map . */ + + int offsetMeasurement; /**< Offset used to fill the measurement vector. */ +}; + +RDE::UkfMeasurement::UkfMeasurement() +{ + m_pimpl = std::make_unique(); +} + +RDE::UkfMeasurement::~UkfMeasurement() = default; + +bool RDE::UkfMeasurement::initialize(std::weak_ptr handler) +{ + constexpr auto logPrefix = "[UkfMeasurement::initialize]"; + + auto ptr = handler.lock(); + if (ptr == nullptr) + { + BipedalLocomotion::log()->error("{} The parameter handler is not valid.", logPrefix); + return false; + } + + if (!ptr->getParameter("sampling_time", m_pimpl->dT)) + { + BipedalLocomotion::log()->error("{} Unable to find the `sampling_time` variable", logPrefix); + return false; + } + + m_pimpl->isInitialized = true; + + return true; +} + +bool RDE::UkfMeasurement::finalize(const System::VariablesHandler& handler) +{ + constexpr auto logPrefix = "[UkfMeasurement::finalize]"; + + if (!m_pimpl->isInitialized) + { + BipedalLocomotion::log()->error("{} Please call initialize() before finalize().", logPrefix); + return false; + } + + m_pimpl->isFinalized = false; + + m_pimpl->measurementSize = 0; + + // finalize all the dynamics + for (auto& [name, dynamics] : m_pimpl->dynamicsList) + { + if(!dynamics->finalize(handler)) + { + BipedalLocomotion::log()->error("{} Error while finalizing the dynamics named {}", logPrefix, name); + return false; + } + + m_pimpl->measurementSize += dynamics->size(); + } + + // Set value of measurement covariance matrix + m_pimpl->covarianceR.resize(m_pimpl->measurementSize, m_pimpl->measurementSize); + + for (auto& [name, dynamics] : m_pimpl->dynamicsList) + { + m_pimpl->measurementVariableHandler.addVariable(name, dynamics->getCovariance().size()); + + m_pimpl->covarianceR.block(m_pimpl->measurementVariableHandler.getVariable(name).offset, m_pimpl->measurementVariableHandler.getVariable(name).offset, + m_pimpl->measurementVariableHandler.getVariable(name).size, m_pimpl->measurementVariableHandler.getVariable(name).size) = dynamics->getCovariance().asDiagonal(); + } + + m_pimpl->jointVelocityState.resize(m_pimpl->kinDynFullModel->model().getNrOfDOFs()); + + m_pimpl->currentState.resize(m_pimpl->measurementSize); + m_pimpl->currentState.setZero(); + + m_pimpl->predictedMeasurement.resize(m_pimpl->measurementSize); + m_pimpl->predictedMeasurement.setZero(); + + m_pimpl->measurement.resize(m_pimpl->measurementSize); + m_pimpl->measurement.setZero(); + + m_pimpl->isFinalized = true; + + return true; +} + +std::unique_ptr RDE::UkfMeasurement::build(std::weak_ptr handler, + System::VariablesHandler& stateVariableHandler, + std::shared_ptr kinDynFullModel, + const std::vector& subModelList, + const std::vector>& kinDynWrapperList) +{ + constexpr auto logPrefix = "[UkfMeasurement::build]"; + + auto ptr = handler.lock(); + if (ptr == nullptr) + { + BipedalLocomotion::log()->error("{} Invalid parameter handler.", logPrefix); + return nullptr; + } + + std::unique_ptr measurement = std::make_unique(); + + if (!measurement->initialize(ptr)) + { + BipedalLocomotion::log()->error("{} Unable to initialize the measurement object of the ukf.", logPrefix); + return nullptr; + } + + std::vector dynamicsList; + if (!ptr->getParameter("dynamics_list", dynamicsList)) + { + BipedalLocomotion::log()->error("{} Unable to find the parameter 'dynamics_list'.", logPrefix); + return nullptr; + } + + if (kinDynFullModel == nullptr) + { + BipedalLocomotion::log()->error("{} The pointer to the `KinDynComputation` object is not valid.", logPrefix); + return nullptr; + } + + measurement->m_pimpl->stateVariableHandler = stateVariableHandler; + + // Set KinDynComputation for the full model + measurement->m_pimpl->kinDynFullModel = kinDynFullModel; + + // Set the list of SubModel + measurement->m_pimpl->subModelList.reserve(subModelList.size()); + measurement->m_pimpl->subModelList = subModelList; + + // set the list of SubModelKinDynWrapper + measurement->m_pimpl->kinDynWrapperList.reserve(kinDynWrapperList.size()); + measurement->m_pimpl->kinDynWrapperList = kinDynWrapperList; + + // per each dynamics add variable to the variableHandler + // and add the dynamics to the list + for (const auto& dynamicsGroupName : dynamicsList) + { + auto dynamicsGroupTmp = ptr->getGroup(dynamicsGroupName).lock(); + if (dynamicsGroupTmp == nullptr) + { + BipedalLocomotion::log()->error("{} Unable to find the group '{}'.", logPrefix, dynamicsGroupName); + return nullptr; + } + + // add dT parameter since it is required by all the dynamics + auto dynamicsGroup = dynamicsGroupTmp->clone(); + dynamicsGroup->setParameter("sampling_time", measurement->m_pimpl->dT); + + // create variable handler + std::string dynamicsName; + std::vector covariances; + std::vector dynamicsElements; + if (!dynamicsGroup->getParameter("name", dynamicsName)) + { + BipedalLocomotion::log()->error("{} Unable to find the parameter 'name'.", logPrefix); + return nullptr; + } + if (!dynamicsGroup->getParameter("covariance", covariances)) + { + BipedalLocomotion::log()->error("{} Unable to find the parameter 'covariance'.", logPrefix); + return nullptr; + } + if (!dynamicsGroup->getParameter("elements", dynamicsElements)) + { + BipedalLocomotion::log()->error("{} Unable to find the parameter 'elements'.", logPrefix); + return nullptr; + } + + std::string dynamicModel; + if (!dynamicsGroup->getParameter("dynamic_model", dynamicModel)) + { + BipedalLocomotion::log()->error("{} Unable to find the parameter 'dynamic_model'.", logPrefix); + return nullptr; + } + + std::shared_ptr dynamicsInstance = RDE::DynamicsFactory::createInstance(dynamicModel); + if (dynamicsInstance == nullptr) + { + BipedalLocomotion::log()->error("{} The dynamic model '{}' has not been registered.", logPrefix, dynamicModel); + return nullptr; + } + + dynamicsInstance->setSubModels(subModelList, kinDynWrapperList); + + dynamicsInstance->initialize(dynamicsGroup); + + // add dynamics to the list + measurement->m_pimpl->dynamicsList.insert({dynamicsName, dynamicsInstance}); + } + + // finalize estimator + if (!measurement->finalize(measurement->m_pimpl->stateVariableHandler)) + { + BipedalLocomotion::log()->error("{} Unable to finalize the RobotDynamicsEstimator.", logPrefix); + return nullptr; + } + + return measurement; +} + +void RDE::UkfMeasurement::setUkfInputProvider(std::shared_ptr ukfInputProvider) +{ + m_pimpl->ukfInputProvider = ukfInputProvider; +} + +std::pair RDE::UkfMeasurement::getNoiseCovarianceMatrix() const +{ + return std::make_pair(true, m_pimpl->covarianceR); +} + +bfl::VectorDescription RDE::UkfMeasurement::getMeasurementDescription() const +{ + return bfl::VectorDescription(m_pimpl->measurementSize); +} + +bfl::VectorDescription RDE::UkfMeasurement::getInputDescription() const +{ + return bfl::VectorDescription(m_pimpl->currentState.size(), 0, m_pimpl->measurementSize); +} + +std::pair RDE::UkfMeasurement::predictedMeasure(const Eigen::Ref& cur_states) const +{ + constexpr auto logPrefix = "[UkfMeasurement::propagate]"; + + // Check that everything is initialized and set + if (!m_pimpl->isFinalized) + { + BipedalLocomotion::log()->error("{} The ukf state is not well initialized.", logPrefix); + throw std::runtime_error("Error"); + } + + if (m_pimpl->ukfInputProvider == nullptr) + { + BipedalLocomotion::log()->error("{} The ukf input provider is not set.", logPrefix); + throw std::runtime_error("Error"); + } + + // Get input of ukf from provider + m_pimpl->ukfInput = m_pimpl->ukfInputProvider->getOutput(); + + m_pimpl->currentState = cur_states; + + m_pimpl->jointVelocityState = m_pimpl->currentState.segment(m_pimpl->stateVariableHandler.getVariable("ds").offset, + m_pimpl->stateVariableHandler.getVariable("ds").size); + + // Update kindyn full model + m_pimpl->kinDynFullModel->setRobotState(m_pimpl->ukfInput.robotBasePose.transform(), + m_pimpl->ukfInput.robotJointPositions, + iDynTree::make_span(m_pimpl->ukfInput.robotBaseVelocity.data(), manif::SE3d::Tangent::DoF), + m_pimpl->jointVelocityState, + m_pimpl->gravity); + + // Update kindyn sub-models + for (int subModelIdx = 0; subModelIdx < m_pimpl->subModelList.size(); subModelIdx++) + { + m_pimpl->kinDynWrapperList[subModelIdx]->updateInternalKinDynState(); + } + + // TODO + // This could be parallelized + + // Update all the dynamics + for (auto& [name, dynamics] : m_pimpl->dynamicsList) + { + dynamics->setState(m_pimpl->currentState); + + dynamics->setInput(m_pimpl->ukfInput); + + if (!dynamics->update()) + { + BipedalLocomotion::log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, name); + throw std::runtime_error("Error"); + } + + m_pimpl->predictedMeasurement.segment(m_pimpl->measurementVariableHandler.getVariable(name).offset, + m_pimpl->measurementVariableHandler.getVariable(name).size) = dynamics->getUpdatedVariable(); + } + + return std::make_pair(true, std::move(m_pimpl->predictedMeasurement)); +} + +std::pair RDE::UkfMeasurement::innovation(const bfl::Data& predicted_measurements, const bfl::Data& measurements) const +{ + Eigen::MatrixXd innovation = -(bfl::any::any_cast(predicted_measurements).colwise() + - bfl::any::any_cast(measurements).col(0)); + + return std::make_pair(true, std::move(innovation)); +} + +std::size_t RDE::UkfMeasurement::getMeasurementSize() +{ + return m_pimpl->measurementSize; +} + +BipedalLocomotion::System::VariablesHandler RDE::UkfMeasurement::getMeasurementVariableHandler() +{ + return m_pimpl->measurementVariableHandler; +} + +void RDE::UkfMeasurement::setStateVariableHandler(System::VariablesHandler stateVariableHandler) const +{ + m_pimpl->stateVariableHandler = stateVariableHandler; +} + +std::pair RDE::UkfMeasurement::measure(const bfl::Data& data) const +{ + return std::make_pair(true, m_pimpl->measurement); +} + +bool RDE::UkfMeasurement::freeze(const bfl::Data& data) +{ + m_pimpl->measurementMap = bfl::any::any_cast>(data); + + for (auto& [name, dynamics] : m_pimpl->dynamicsList) + { + m_pimpl->offsetMeasurement = m_pimpl->measurementVariableHandler.getVariable(name).offset; + + while(m_pimpl->offsetMeasurement < (m_pimpl->measurementVariableHandler.getVariable(name).offset + m_pimpl->measurementVariableHandler.getVariable(name).size)) + { + m_pimpl->measurement.segment(m_pimpl->offsetMeasurement, m_pimpl->measurementMap[name].size()) + = m_pimpl->measurementMap[name]; + + m_pimpl->offsetMeasurement += m_pimpl->measurementMap[name].size(); + } + } + + return true; +} diff --git a/src/Estimators/src/UkfState.cpp b/src/Estimators/src/UkfState.cpp index c68eaf776e..4940d5f2a4 100644 --- a/src/Estimators/src/UkfState.cpp +++ b/src/Estimators/src/UkfState.cpp @@ -95,7 +95,7 @@ bool RDE::UkfState::finalize(const System::VariablesHandler& handler) { if(!dynamics->finalize(handler)) { - log()->error("{} Error while finalizing the dynamics names {}", logPrefix, name); + log()->error("{} Error while finalizing the dynamics named {}", logPrefix, name); return false; } @@ -144,7 +144,7 @@ std::unique_ptr RDE::UkfState::build(std::weak_ptrinitialize(ptr)) { - log()->error("{} Unable to initialize the RobotDynamicsEstimator.", logPrefix); + log()->error("{} Unable to initialize the state object of the ukf.", logPrefix); return nullptr; } diff --git a/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp new file mode 100644 index 0000000000..ea22d24c68 --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp @@ -0,0 +1,229 @@ +/** + * @file AccelerometerMeasurementDynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; + +void createModelLoader(IParametersHandler::shared_ptr group, + iDynTree::ModelLoader& mdlLdr) +{ + // List of joints and fts to load the model + std::vector subModelList; + + const std::string modelPath = iCubModels::getModelFile("iCubGenova09"); + + std::vector jointList; + REQUIRE(group->getParameter("joint_list", jointList)); + + std::vector ftFramesList; + auto ftGroup = group->getGroup("FT").lock(); + REQUIRE(ftGroup->getParameter("frames", ftFramesList)); + + std::vector jointsAndFTs; + jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); +// jointsAndFTs.insert(jointsAndFTs.end(), ftFramesList.begin(), ftFramesList.end()); + + REQUIRE(mdlLdr.loadReducedModelFromFile(modelPath, jointsAndFTs)); +} + +void createSubModels(iDynTree::ModelLoader& mdlLdr, + std::shared_ptr kinDynFullModel, + IParametersHandler::shared_ptr group, + SubModelCreator& subModelCreator) +{ + subModelCreator.setModelAndSensors(mdlLdr.model(), mdlLdr.sensors()); + REQUIRE(subModelCreator.setKinDyn(kinDynFullModel)); + + REQUIRE(subModelCreator.createSubModels(group)); +} + +bool setStaticState(std::shared_ptr kinDyn) +{ + size_t dofs = kinDyn->getNrOfDegreesOfFreedom(); + iDynTree::Transform worldTbase; + Eigen::VectorXd baseVel(6); + Eigen::Vector3d gravity; + + Eigen::VectorXd qj(dofs), dqj(dofs), ddqj(dofs); + + worldTbase = iDynTree::Transform(iDynTree::Rotation::Identity(), iDynTree::Position::Zero()); + + Eigen::Matrix4d transform = toEigen(worldTbase.asHomogeneousTransform()); + + gravity.setZero(); + gravity(2) = - BipedalLocomotion::Math::StandardAccelerationOfGravitation; + + baseVel.setZero(); + + for (size_t dof = 0; dof < dofs; dof++) + { + qj(dof) = 0.0; + dqj(dof) = 0.0; + ddqj(dof) = 0.0; + } + + return kinDyn->setRobotState(transform, + iDynTree::make_span(qj.data(), qj.size()), + iDynTree::make_span(baseVel.data(), baseVel.size()), + iDynTree::make_span(dqj.data(), dqj.size()), + iDynTree::make_span(gravity.data(), gravity.size())); +} + +TEST_CASE("Friction Torque Dynamics") +{ + // Create parameter handler + auto parameterHandler = std::make_shared(); + + const std::string name = "r_leg_ft_acc"; + Eigen::VectorXd covariance(3); + covariance << 2.3e-3, 1.9e-3, 3.1e-3; + const std::string model = "AccelerometerMeasurementDynamics"; + const bool useBias = true; + + double dT = 0.01; + + const std::vector jointList = {"r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll"}; + + parameterHandler->setParameter("name", name); + parameterHandler->setParameter("covariance", covariance); + parameterHandler->setParameter("dynamic_model", model); + parameterHandler->setParameter("sampling_time", dT); + parameterHandler->setParameter("use_bias", useBias); + + // Create state 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("r_leg_ft_acc_bias", 3)); + + // Create model variable handler to load the robot model + auto modelParamHandler = std::make_shared(); + auto emptyGroupNamesFrames = std::make_shared(); + std::vector emptyVectorString; + emptyGroupNamesFrames->setParameter("names", emptyVectorString); + emptyGroupNamesFrames->setParameter("frames", emptyVectorString); + REQUIRE(modelParamHandler->setGroup("FT", emptyGroupNamesFrames)); + REQUIRE(modelParamHandler->setGroup("GYROSCOPE", emptyGroupNamesFrames)); + + auto accGroup = std::make_shared(); + std::vector accNameList = {"r_leg_ft_acc"}; + std::vector accFrameList = {"r_leg_ft_sensor"}; + accGroup->setParameter("names", accNameList); + accGroup->setParameter("frames", accFrameList); + REQUIRE(modelParamHandler->setGroup("ACCELEROMETER", accGroup)); + + auto emptyGroupFrames = std::make_shared(); + emptyGroupFrames->setParameter("frames", emptyVectorString); + REQUIRE(modelParamHandler->setGroup("EXTERNAL_CONTACT", emptyGroupFrames)); + + modelParamHandler->setParameter("joint_list", jointList); + + // Load model + iDynTree::ModelLoader modelLoader; + createModelLoader(modelParamHandler, modelLoader); + + auto kinDyn = std::make_shared(); + REQUIRE(kinDyn->loadRobotModel(modelLoader.model())); + REQUIRE(kinDyn->setFrameVelocityRepresentation(iDynTree::BODY_FIXED_REPRESENTATION)); + + REQUIRE(setStaticState(kinDyn)); + + SubModelCreator subModelCreator; + createSubModels(modelLoader, kinDyn, modelParamHandler, subModelCreator); + + std::vector> kinDynWrapperList; + const auto & subModelList = subModelCreator.getSubModelList(); + + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + kinDynWrapperList.emplace_back(std::make_shared()); + REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); + REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); + REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState()); + } + + AccelerometerMeasurementDynamics accDynamics; + REQUIRE(accDynamics.setSubModels(subModelList, kinDynWrapperList)); + REQUIRE(accDynamics.initialize(parameterHandler)); + REQUIRE(accDynamics.finalize(variableHandler)); + + manif::SE3d::Tangent robotBaseAcceleration; + robotBaseAcceleration.setZero(); + + Eigen::VectorXd robotJointAcceleration(kinDyn->model().getNrOfDOFs()); + robotJointAcceleration.setZero(); + + iDynTree::LinkNetExternalWrenches extWrench(kinDyn->model()); + extWrench.zero(); + + // Compute joint torques in static configuration from inverse dynamics on the full model + iDynTree::FreeFloatingGeneralizedTorques jointTorques(kinDyn->model()); + + kinDyn->inverseDynamics(iDynTree::make_span(robotBaseAcceleration.data(), + manif::SE3d::Tangent::DoF), + robotJointAcceleration, + extWrench, + jointTorques); + + Eigen::VectorXd state; + state.resize(variableHandler.getNumberOfVariables()); + state.setZero(); + + int offset = variableHandler.getVariable("tau_m").offset; + int size = variableHandler.getVariable("tau_m").size; + for (int jointIndex = 0; jointIndex < size; jointIndex++) + { + state[offset+jointIndex] = jointTorques.jointTorques()[jointIndex]; + } + + // Create an input for the ukf state + UKFInput input; + + // Define joint positions + Eigen::VectorXd jointPos; + jointPos.resize(kinDyn->model().getNrOfDOFs()); + jointPos.setZero(); + input.robotJointPositions = jointPos; + + // Define base pose + manif::SE3d basePose; + basePose.setIdentity(); + input.robotBasePose = basePose; + + // Define base velocity and acceleration + manif::SE3d::Tangent baseVelocity, baseAcceleration; + baseVelocity.setZero(); + baseAcceleration.setZero(); + input.robotBaseVelocity = baseVelocity; + input.robotBaseAcceleration = baseAcceleration; + + accDynamics.setInput(input); + accDynamics.setState(state); + + REQUIRE(accDynamics.update()); + for (int idx = 0; idx < accDynamics.getUpdatedVariable().size(); idx++) + { + REQUIRE(std::abs(accDynamics.getUpdatedVariable()(idx)) < 10); + } +} diff --git a/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt b/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt index 1bd07a78ea..f3ddf4d7b4 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt +++ b/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt @@ -53,12 +53,40 @@ if(FRAMEWORK_USE_icub-models AND FRAMEWORK_COMPILE_YarpImplementation) MANIF::manif ) + add_bipedal_test(NAME AccelerometerMeasurementDynamics + SOURCES AccelerometerMeasurementDynamicsTest.cpp + LINKS BipedalLocomotion::RobotDynamicsEstimator + BipedalLocomotion::ParametersHandler + BipedalLocomotion::ParametersHandlerYarpImplementation + BipedalLocomotion::System + icub-models::icub-models + Eigen3::Eigen + MANIF::manif + ) + + add_bipedal_test(NAME GyroscopeMeasurementDynamics + SOURCES GyroscopeMeasurementDynamicsTest.cpp + LINKS BipedalLocomotion::RobotDynamicsEstimator + BipedalLocomotion::ParametersHandler + BipedalLocomotion::ParametersHandlerYarpImplementation + BipedalLocomotion::System + icub-models::icub-models + Eigen3::Eigen + MANIF::manif + ) + add_bipedal_test(NAME UkfState SOURCES UkfStateTest.cpp LINKS BipedalLocomotion::RobotDynamicsEstimator BipedalLocomotion::ManifConversions BipedalLocomotion::ParametersHandlerYarpImplementation icub-models::icub-models ) + add_bipedal_test(NAME UkfMeasurement + SOURCES UkfMeasurementTest.cpp + LINKS BipedalLocomotion::RobotDynamicsEstimator BipedalLocomotion::ManifConversions BipedalLocomotion::ParametersHandlerYarpImplementation + icub-models::icub-models + ) + include_directories(${CMAKE_CURRENT_BINARY_DIR}) configure_file("${CMAKE_CURRENT_SOURCE_DIR}/ConfigFolderPath.h.in" "${CMAKE_CURRENT_BINARY_DIR}/ConfigFolderPath.h" @ONLY) diff --git a/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp index fa9968c9de..2cee47a5b0 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp @@ -193,7 +193,6 @@ TEST_CASE("Friction Torque Dynamics") state[offset+jointIndex] = jointTorques.jointTorques()[jointIndex]; } - // Test with state set to zero. tau_F_dynamics.setState(state); REQUIRE(tau_F_dynamics.update()); for (int idx = 0; idx < tau_F_dynamics.getUpdatedVariable().size(); idx++) diff --git a/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp new file mode 100644 index 0000000000..07d4c351bf --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp @@ -0,0 +1,207 @@ +/** + * @file GyroscopeMeasurementDynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; + +void createModelLoader(IParametersHandler::shared_ptr group, + iDynTree::ModelLoader& mdlLdr) +{ + // List of joints and fts to load the model + std::vector subModelList; + + const std::string modelPath = iCubModels::getModelFile("iCubGenova09"); + + std::vector jointList; + REQUIRE(group->getParameter("joint_list", jointList)); + + std::vector ftFramesList; + auto ftGroup = group->getGroup("FT").lock(); + REQUIRE(ftGroup->getParameter("frames", ftFramesList)); + + std::vector jointsAndFTs; + jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); +// jointsAndFTs.insert(jointsAndFTs.end(), ftFramesList.begin(), ftFramesList.end()); + + REQUIRE(mdlLdr.loadReducedModelFromFile(modelPath, jointsAndFTs)); +} + +void createSubModels(iDynTree::ModelLoader& mdlLdr, + std::shared_ptr kinDynFullModel, + IParametersHandler::shared_ptr group, + SubModelCreator& subModelCreator) +{ + subModelCreator.setModelAndSensors(mdlLdr.model(), mdlLdr.sensors()); + REQUIRE(subModelCreator.setKinDyn(kinDynFullModel)); + + REQUIRE(subModelCreator.createSubModels(group)); +} + +bool setStaticState(std::shared_ptr kinDyn) +{ + size_t dofs = kinDyn->getNrOfDegreesOfFreedom(); + iDynTree::Transform worldTbase; + Eigen::VectorXd baseVel(6); + Eigen::Vector3d gravity; + + Eigen::VectorXd qj(dofs), dqj(dofs), ddqj(dofs); + + worldTbase = iDynTree::Transform(iDynTree::Rotation::Identity(), iDynTree::Position::Zero()); + + Eigen::Matrix4d transform = toEigen(worldTbase.asHomogeneousTransform()); + + gravity.setZero(); + gravity(2) = - BipedalLocomotion::Math::StandardAccelerationOfGravitation; + + baseVel.setZero(); + + for (size_t dof = 0; dof < dofs; dof++) + { + qj(dof) = 0.0; + dqj(dof) = 0.0; + ddqj(dof) = 0.0; + } + + return kinDyn->setRobotState(transform, + iDynTree::make_span(qj.data(), qj.size()), + iDynTree::make_span(baseVel.data(), baseVel.size()), + iDynTree::make_span(dqj.data(), dqj.size()), + iDynTree::make_span(gravity.data(), gravity.size())); +} + +TEST_CASE("Gyroscope Measurement Dynamics") +{ + // Create parameter handler + auto parameterHandler = std::make_shared(); + + const std::string name = "r_leg_ft_gyro"; + Eigen::VectorXd covariance(3); + covariance << 2.3e-3, 1.9e-3, 3.1e-3; + const std::string model = "GyroscopeMeasurementDynamics"; + const bool useBias = true; + + double dT = 0.01; + + const std::vector jointList = {"r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll"}; + + parameterHandler->setParameter("name", name); + parameterHandler->setParameter("covariance", covariance); + parameterHandler->setParameter("dynamic_model", model); + parameterHandler->setParameter("sampling_time", dT); + parameterHandler->setParameter("use_bias", useBias); + + // Create state 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("r_leg_ft_gyro_bias", 3)); + + // Create model variable handler to load the robot model + auto modelParamHandler = std::make_shared(); + auto emptyGroupNamesFrames = std::make_shared(); + std::vector emptyVectorString; + emptyGroupNamesFrames->setParameter("names", emptyVectorString); + emptyGroupNamesFrames->setParameter("frames", emptyVectorString); + REQUIRE(modelParamHandler->setGroup("FT", emptyGroupNamesFrames)); + REQUIRE(modelParamHandler->setGroup("ACCELEROMETER", emptyGroupNamesFrames)); + + auto accGroup = std::make_shared(); + std::vector accNameList = {"r_leg_ft_gyro"}; + std::vector accFrameList = {"r_leg_ft_sensor"}; + accGroup->setParameter("names", accNameList); + accGroup->setParameter("frames", accFrameList); + REQUIRE(modelParamHandler->setGroup("GYROSCOPE", accGroup)); + + auto emptyGroupFrames = std::make_shared(); + emptyGroupFrames->setParameter("frames", emptyVectorString); + REQUIRE(modelParamHandler->setGroup("EXTERNAL_CONTACT", emptyGroupFrames)); + + modelParamHandler->setParameter("joint_list", jointList); + + // Load model + iDynTree::ModelLoader modelLoader; + createModelLoader(modelParamHandler, modelLoader); + + auto kinDyn = std::make_shared(); + REQUIRE(kinDyn->loadRobotModel(modelLoader.model())); + REQUIRE(kinDyn->setFrameVelocityRepresentation(iDynTree::BODY_FIXED_REPRESENTATION)); + + REQUIRE(setStaticState(kinDyn)); + + SubModelCreator subModelCreator; + createSubModels(modelLoader, kinDyn, modelParamHandler, subModelCreator); + + std::vector> kinDynWrapperList; + const auto & subModelList = subModelCreator.getSubModelList(); + + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + kinDynWrapperList.emplace_back(std::make_shared()); + REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); + REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); + REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState()); + } + + GyroscopeMeasurementDynamics gyroDynamics; + REQUIRE(gyroDynamics.setSubModels(subModelList, kinDynWrapperList)); + REQUIRE(gyroDynamics.initialize(parameterHandler)); + REQUIRE(gyroDynamics.finalize(variableHandler)); + + manif::SE3d::Tangent robotBaseAcceleration; + robotBaseAcceleration.setZero(); + + Eigen::VectorXd robotJointAcceleration(kinDyn->model().getNrOfDOFs()); + robotJointAcceleration.setZero(); + + iDynTree::LinkNetExternalWrenches extWrench(kinDyn->model()); + extWrench.zero(); + + // Compute joint torques in static configuration from inverse dynamics on the full model + iDynTree::FreeFloatingGeneralizedTorques jointTorques(kinDyn->model()); + + kinDyn->inverseDynamics(iDynTree::make_span(robotBaseAcceleration.data(), + manif::SE3d::Tangent::DoF), + robotJointAcceleration, + extWrench, + jointTorques); + + Eigen::VectorXd state; + state.resize(variableHandler.getNumberOfVariables()); + state.setZero(); + + int offset = variableHandler.getVariable("tau_m").offset; + int size = variableHandler.getVariable("tau_m").size; + for (int jointIndex = 0; jointIndex < size; jointIndex++) + { + state[offset+jointIndex] = jointTorques.jointTorques()[jointIndex]; + } + + gyroDynamics.setState(state); + + REQUIRE(gyroDynamics.update()); + for (int idx = 0; idx < gyroDynamics.getUpdatedVariable().size(); idx++) + { + REQUIRE(std::abs(gyroDynamics.getUpdatedVariable()(idx)) < 0.1); + } +} diff --git a/src/Estimators/tests/RobotDynamicsEstimator/UkfMeasurementTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/UkfMeasurementTest.cpp new file mode 100644 index 0000000000..a757049f89 --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/UkfMeasurementTest.cpp @@ -0,0 +1,252 @@ +/** + * @file UkfMeasurementTest.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include +#include + +//BLF +#include +#include +#include + +// iDynTree +#include +#include + +//LIBRARYTOTEST +#include +#include +#include +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +IParametersHandler::shared_ptr loadParameterHandler() +{ + std::shared_ptr originalHandler = std::make_shared(); + IParametersHandler::shared_ptr parameterHandler = originalHandler; + + yarp::os::ResourceFinder& rf = yarp::os::ResourceFinder::getResourceFinderSingleton(); + rf.setDefaultConfigFile("/config/config.ini"); + + std::vector arguments = {" ", "--from ", getConfigPath()}; + + std::vector argv; + for (const auto& arg : arguments) + argv.push_back((char*)arg.data()); + argv.push_back(nullptr); + + rf.configure(argv.size() - 1, argv.data()); + + REQUIRE_FALSE(rf.isNull()); + parameterHandler->clear(); + REQUIRE(parameterHandler->isEmpty()); + + originalHandler->set(rf); + + return parameterHandler; +} + +void createModelLoader(IParametersHandler::weak_ptr group, + iDynTree::ModelLoader& mdlLdr) +{ + // List of joints and fts to load the model + std::vector subModelList; + + const std::string modelPath = iCubModels::getModelFile("iCubGenova09"); + + std::vector jointList; + REQUIRE(group.lock()->getParameter("joint_list", jointList)); + + std::vector ftFramesList; + auto ftGroup = group.lock()->getGroup("FT").lock(); + REQUIRE(ftGroup->getParameter("frames", ftFramesList)); + + std::vector jointsAndFTs; + jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); + jointsAndFTs.insert(jointsAndFTs.end(), ftFramesList.begin(), ftFramesList.end()); + + REQUIRE(mdlLdr.loadReducedModelFromFile(modelPath, jointsAndFTs)); +} + +TEST_CASE("UkfMeasurement") +{ + auto parameterHandler = loadParameterHandler(); + + iDynTree::ModelLoader modelLoader; + auto modelHandler = parameterHandler->getGroup("MODEL").lock(); + + REQUIRE_FALSE(modelHandler == nullptr); + + createModelLoader(modelHandler, modelLoader); + + auto kinDyn = std::make_shared(); + REQUIRE(kinDyn->loadRobotModel(modelLoader.model())); + + std::vector, std::shared_ptr>> subModelMap; + + SubModelCreator subModelCreator; + subModelCreator.setModelAndSensors(kinDyn->model(), modelLoader.sensors()); + REQUIRE(subModelCreator.setKinDyn(kinDyn)); + + REQUIRE(subModelCreator.createSubModels(modelHandler)); + + std::vector> kinDynWrapperList; + + const auto & subModelList = subModelCreator.getSubModelList(); + + for (int idx = 0; idx < subModelList.size(); idx++) + { + kinDynWrapperList.emplace_back(std::make_shared()); + REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); + REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); + REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState()); + } + + // Build the UkfState + auto groupUKF = parameterHandler->getGroup("UKF").lock(); + REQUIRE_FALSE(groupUKF == nullptr); + + auto groupUKFMeasurementTmp = groupUKF->getGroup("UKF_MEASUREMENT").lock(); + REQUIRE_FALSE(groupUKFMeasurementTmp == nullptr); + + auto groupUKFMeasurement = groupUKFMeasurementTmp->clone(); + double dT; + REQUIRE(parameterHandler->getGroup("GENERAL").lock()->getParameter("sampling_time", dT)); + groupUKFMeasurement->setParameter("sampling_time", dT); + + // Create the 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("r_leg_ft_sensor", 6)); + REQUIRE(stateVariableHandler.addVariable("r_foot_front_ft_sensor", 6)); + REQUIRE(stateVariableHandler.addVariable("r_foot_rear_ft_sensor", 6)); + REQUIRE(stateVariableHandler.addVariable("r_leg_ft_sensor_bias", 6)); + REQUIRE(stateVariableHandler.addVariable("r_foot_front_ft_sensor_bias", 6)); + REQUIRE(stateVariableHandler.addVariable("r_foot_rear_ft_sensor_bias", 6)); + REQUIRE(stateVariableHandler.addVariable("r_leg_ft_acc_bias", 3)); + REQUIRE(stateVariableHandler.addVariable("r_foot_front_ft_acc_bias", 3)); + REQUIRE(stateVariableHandler.addVariable("r_foot_rear_ft_acc_bias", 3)); + REQUIRE(stateVariableHandler.addVariable("r_leg_ft_gyro_bias", 3)); + REQUIRE(stateVariableHandler.addVariable("r_foot_front_ft_gyro_bias", 3)); + REQUIRE(stateVariableHandler.addVariable("r_foot_rear_ft_gyro_bias", 3)); + + std::unique_ptr measurementModel = UkfMeasurement::build(groupUKFMeasurement, + stateVariableHandler, + kinDyn, + subModelList, + kinDynWrapperList); + + REQUIRE(measurementModel != nullptr); + + // Create an input for the ukf state + UKFInput input; + + // Define joint positions + Eigen::VectorXd jointPos; + jointPos.resize(kinDyn->model().getNrOfDOFs()); + jointPos.setZero(); + input.robotJointPositions = jointPos; + + // Define base pose + manif::SE3d basePose; + basePose.setIdentity(); + input.robotBasePose = basePose; + + // Define base velocity and acceleration + manif::SE3d::Tangent baseVelocity, baseAcceleration; + baseVelocity.setZero(); + baseAcceleration.setZero(); + input.robotBaseVelocity = baseVelocity; + input.robotBaseAcceleration = baseAcceleration; + + std::shared_ptr inputProvider = std::make_shared(); + + BipedalLocomotion::System::VariablesHandler measurementHandler = measurementModel->getMeasurementVariableHandler(); +// int measurementSize = measurementModel->getMeasurementSize(); + int stateSize = stateVariableHandler.getNumberOfVariables(); + + measurementModel->setUkfInputProvider(inputProvider); + + Eigen::VectorXd currentState; + currentState.resize(stateSize); + currentState.setZero(); + + Eigen::VectorXd motorTorques; + motorTorques.resize(kinDyn->model().getNrOfDOFs()); + motorTorques << -1.6298, -1.10202, 0, -0.74, 0.0877, -0.00173; + Eigen::VectorXd wrenchFTtLeg; + wrenchFTtLeg.resize(6); + wrenchFTtLeg << 0, 0, 100.518, 0.748, 0.91, 0; + Eigen::VectorXd wrenchFTFootFront; + wrenchFTFootFront.resize(6); + wrenchFTFootFront << 0, 0, 1.761, -0.001, 0.0003, 0; + Eigen::VectorXd wrenchFTFootRear; + wrenchFTFootRear.resize(6); + wrenchFTFootRear << 0, 0, 1.752, 0.000876, 0.000649, 0; + + currentState.segment(stateVariableHandler.getVariable("tau_m").offset, stateVariableHandler.getVariable("tau_m").size) = motorTorques; + currentState.segment(stateVariableHandler.getVariable("r_leg_ft_sensor").offset, stateVariableHandler.getVariable("r_leg_ft_sensor").size) = wrenchFTtLeg; + currentState.segment(stateVariableHandler.getVariable("r_foot_front_ft_sensor").offset, stateVariableHandler.getVariable("r_foot_front_ft_sensor").size) = wrenchFTFootFront; + currentState.segment(stateVariableHandler.getVariable("r_foot_rear_ft_sensor").offset, stateVariableHandler.getVariable("r_foot_rear_ft_sensor").size) = wrenchFTFootRear; + currentState.segment(stateVariableHandler.getVariable("r_foot_rear_ft_sensor").offset, stateVariableHandler.getVariable("r_foot_rear_ft_sensor").size) = wrenchFTFootRear; + + REQUIRE(inputProvider->setInput(input)); + + std::map measurement; + + Eigen::VectorXd i_m; + i_m.resize(kinDyn->model().getNrOfDOFs()); + i_m << -0.1468, 0.2345, 0, -0.0667, 0.008, -0.000692; + measurement["i_m"] = i_m; + measurement["r_leg_ft_sensor"] = wrenchFTtLeg; + measurement["r_foot_front_ft_sensor"] = wrenchFTFootFront; + measurement["r_foot_rear_ft_sensor"] = wrenchFTFootRear; + + Eigen::Vector3d zeroVec3; + zeroVec3.setZero(); + + measurement["r_leg_ft_gyro"] = zeroVec3; + measurement["r_foot_front_ft_gyro"] = zeroVec3; + measurement["r_foot_rear_ft_gyro"] = zeroVec3; + + Eigen::Vector3d gravVec; + gravVec.setZero(); + gravVec(2) = 9.8066; + + measurement["r_leg_ft_acc"] = gravVec; + measurement["r_foot_front_ft_acc"] = gravVec; + measurement["r_foot_rear_ft_acc"] = gravVec; + + Eigen::VectorXd zeroVector6(stateVariableHandler.getVariable("tau_m").size); + zeroVector6.setZero(); + measurement["ds"] = zeroVector6; + + bfl::Data updatedMeasurementTmp; + + bool resultOk; + + REQUIRE(measurementModel->freeze(measurement)); + + std::tie(resultOk, updatedMeasurementTmp) = measurementModel->predictedMeasure(currentState); + + REQUIRE(resultOk); + + Eigen::VectorXd updatedMeasurement = bfl::any::any_cast(std::move(updatedMeasurementTmp)); + + for (int idx = 0; idx < updatedMeasurement.size(); idx++) + { + REQUIRE(std::abs(updatedMeasurement[idx]) < 200); + } +} diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini index fcd7b21691..db42018a42 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini @@ -1,46 +1,71 @@ -# The state list should match the group names, otherwise the states are not loaded in the ukf model -measurement_list ("JOINT_VELOCITY", - "MOTOR_CURRENT", - "FT", - "ACCELEROMETER", - "GYROSCOPE") +dynamics_list ("JOINT_VELOCITY", "MOTOR_CURRENT", "RIGHT_LEG_FT", "RIGHT_FOOT_FRONT_FT", "RIGHT_FOOT_REAR_FT", "RIGHT_LEG_ACC", "RIGHT_LEG_GYRO") [JOINT_VELOCITY] -name ("ds") +name "ds" elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") covariance (1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4) -dynamic_model "LinearModel" +dynamic_model "ZeroVelocityDynamics" [MOTOR_CURRENT] -name ("i_m") +name "i_m" elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") covariance (7.5e-4, 7.6e-4, 1.6e-3, 1.4e-3, 1.3e-4, 7.4e-4) -dynamic_model "MotorCurrentModel" +gear_ratio (100.0, -100.0, 100.0, 100.0, 100.0, 100.0) +torque_constant (0.111, 0.047, 0.047, 0.111, 0.111, 0.025) +dynamic_model "MotorCurrentMeasurementDynamics" -[FT] -name ("r_leg_ft_sensor", "r_foot_front_ft_sensor", "r_foot_rear_ft_sensor") +[RIGHT_LEG_FT] +name "r_leg_ft_sensor" elements ("fx", "fy", "fz", "mx", "my", "mz") -covariance (1e-2, 1e-2, 1e-2, 1e-3, 1e-3, 1e-3, - 1e-2, 1e-2, 1e-2, 1e-5, 1e-5, 1e-5, - 1e-2, 1e-2, 1e-2, 1e-5, 1e-5, 1e-5) +covariance (1e-2, 1e-2, 1e-2, 1e-3, 1e-3, 1e-3) use_bias 1 -dynamic_model "LinearModel" +dynamic_model "ZeroVelocityDynamics" -[ACCELEROMETER] -name ("r_leg_ft_acc", "r_foot_front_ft_acc", "r_foot_rear_ft_acc") +[RIGHT_FOOT_FRONT_FT] +name "r_foot_front_ft_sensor" +elements ("fx", "fy", "fz", "mx", "my", "mz") +covariance (1e-2, 1e-2, 1e-2, 1e-5, 1e-5, 1e-5) +use_bias 1 +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_FOOT_REAR_FT] +name "r_foot_rear_ft_sensor" +elements ("fx", "fy", "fz", "mx", "my", "mz") +covariance (1e-2, 1e-2, 1e-2, 1e-5, 1e-5, 1e-5) +use_bias 1 +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_LEG_ACC] +name "r_leg_ft_acc" elements ("x", "y", "z") -covariance (2.3e-3, 1.9e-3, 3.1e-3, - 2.7e-3, 1.99e-3, 2.78e-3, - 2.5e-3, 2.3e-3, 3e-3) +covariance (2.3e-3, 1.9e-3, 3.1e-3) use_bias 1 -dynamic_model "AccelerometerModel" +dynamic_model "AccelerometerMeasurementDynamics" -[GYROSCOPE] -name ("r_leg_ft_gyro", "r_foot_front_ft_gyro", "r_foot_rear_ft_gyro") +[RIGHT_LEG_GYRO] +name "r_leg_ft_gyro" elements ("x", "y", "z") -covariance (0.000000283, 0.0127, 0.0000001, - 4.1e-4, 3.3e-4, 3.2e-4, - 4.9e-4, 4.2e-4, 3.3e-4) +covariance (0.000000283, 0.0127, 0.0000001) use_bias 1 -dynamic_model "GyroscopeModel" +dynamic_model "GyroscopeMeasurementDynamics" + +#[ACCELEROMETER] +#name ("r_leg_ft_acc", "r_foot_front_ft_acc", "r_foot_rear_ft_acc") +#elements ("x", "y", "z") +#covariance (2.3e-3, 1.9e-3, 3.1e-3, +# 2.7e-3, 1.99e-3, 2.78e-3, +# 2.5e-3, 2.3e-3, 3e-3) +#use_bias 1 +#dynamic_model "AccelerometerMeasurementDynamics" + +#[GYROSCOPE] +#name ("r_leg_ft_gyro", "r_foot_front_ft_gyro", "r_foot_rear_ft_gyro") +#elements ("x", "y", "z") +#covariance (0.000000283, 0.0127, 0.0000001, +# 4.1e-4, 3.3e-4, 3.2e-4, +# 4.9e-4, 4.2e-4, 3.3e-4) +#use_bias 1 +#dynamic_model "GyroscopeMeasurementDynamics" +# "RIGHT_LEG_ACC", "RIGHT_FOOT_FRONT_ACC", "RIGHT_FOOT_REAR_ACC", "RIGHT_LEG_GYRO", +# "RIGHT_FOOT_FRONT_GYRO", "RIGHT_FOOT_REAR_GYRO") From 90bbb1b4ca4978648cca3fa1d676edc3d6389bc7 Mon Sep 17 00:00:00 2001 From: Ines Date: Thu, 30 Mar 2023 16:45:30 +0200 Subject: [PATCH 04/13] Update CHANGELOG.md --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index b77688cf56..77bab0dce7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,7 @@ All notable changes to this project are documented in this file. - Add the possibility to retrieve and set duration from the `IParametersHandler` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) - Add the possibility to update the contact list in the swing foot trajectory planner (https://github.com/ami-iit/bipedal-locomotion-framework/pull/637) - Implement state dynamic models needed to define an UKF process model (https://github.com/ami-iit/bipedal-locomotion-framework/pull/615) +- Implement measurement dynamic models needed to define an UKF measurement model (https://github.com/ami-iit/bipedal-locomotion-framework/pull/640) ### Changed - Update the `IK tutorial` to use `QPInverseKinematics::build` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/621) From 999feaf542f886bb3fedac2676720b2a7f989317 Mon Sep 17 00:00:00 2001 From: Ines Date: Mon, 3 Apr 2023 19:13:18 +0200 Subject: [PATCH 05/13] Implement RobotDynamicsEstimator --- src/Estimators/CMakeLists.txt | 3 +- .../RobotDynamicsEstimator/Dynamics.h | 9 +- .../FrictionTorqueStateDynamics.h | 1 + .../JointVelocityStateDynamics.h | 2 +- .../RobotDynamicsEstimator.h | 170 +++++++ .../SubModelKinDynWrapper.h | 4 +- .../RobotDynamicsEstimator/UkfMeasurement.h | 2 +- .../RobotDynamicsEstimator/UkfState.h | 12 +- .../ZeroVelocityDynamics.h | 1 + src/Estimators/src/Dynamics.cpp | 5 + .../src/FrictionTorqueStateDynamics.cpp | 9 + .../src/JointVelocityStateDynamics.cpp | 7 + src/Estimators/src/RobotDynamicsEstimator.cpp | 446 ++++++++++++++++++ src/Estimators/src/SubModel.cpp | 1 - src/Estimators/src/SubModelDynamics.cpp | 10 +- src/Estimators/src/SubModelKinDynWrapper.cpp | 79 ++-- src/Estimators/src/UkfMeasurement.cpp | 95 ++-- src/Estimators/src/UkfState.cpp | 89 ++-- src/Estimators/src/ZeroVelocityDynamics.cpp | 10 +- .../AccelerometerMeasurementDynamicsTest.cpp | 2 +- .../RobotDynamicsEstimator/CMakeLists.txt | 29 +- .../FrictionTorqueStateDynamicsTest.cpp | 26 +- .../GyroscopeMeasurementDynamicsTest.cpp | 25 +- .../JointVelocityStateDynamicsTest.cpp | 222 +++++++-- .../RobotDynamicsEstimatorTest.cpp | 286 +++++++++++ .../SubModelKinDynWrapperTest.cpp | 2 +- .../UkfMeasurementTest.cpp | 4 +- .../RobotDynamicsEstimator/UkfStateTest.cpp | 2 +- .../ZeroVelocityDynamicsTest.cpp | 2 + .../RobotDynamicsEstimator/config/ukf.ini | 4 + .../config/ukf/ukf_measurement.ini | 50 +- .../config/ukf/ukf_state.ini | 17 +- 32 files changed, 1415 insertions(+), 211 deletions(-) create mode 100644 src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/RobotDynamicsEstimator.h create mode 100644 src/Estimators/src/RobotDynamicsEstimator.cpp create mode 100644 src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp diff --git a/src/Estimators/CMakeLists.txt b/src/Estimators/CMakeLists.txt index 2e0ccff031..995269e7b3 100644 --- a/src/Estimators/CMakeLists.txt +++ b/src/Estimators/CMakeLists.txt @@ -30,11 +30,12 @@ if(FRAMEWORK_COMPILE_RobotDynamicsEstimator) SOURCES src/SubModel.cpp src/SubModelKinDynWrapper.cpp src/Dynamics.cpp src/ZeroVelocityDynamics.cpp src/FrictionTorqueStateDynamics.cpp src/JointVelocityStateDynamics.cpp src/UkfState.cpp src/SubModelDynamics.cpp src/MotorCurrentMeasurementDynamics.cpp src/UkfMeasurement.cpp src/AccelerometerMeasurementDynamics.cpp src/GyroscopeMeasurementDynamics.cpp + src/RobotDynamicsEstimator.cpp SUBDIRECTORIES tests/RobotDynamicsEstimator PUBLIC_HEADERS ${H_PREFIX}/SubModel.h ${H_PREFIX}/SubModelKinDynWrapper.h ${H_PREFIX}/Dynamics.h ${H_PREFIX}/ZeroVelocityDynamics.h ${H_PREFIX}/FrictionTorqueStateDynamics.h ${H_PREFIX}/JointVelocityStateDynamics.h ${H_PREFIX}/AccelerometerMeasurementDynamics.h ${H_PREFIX}/UkfState.h ${H_PREFIX}/SubModelDynamics.h ${H_PREFIX}/MotorCurrentMeasurementDynamics.h ${H_PREFIX}/UkfMeasurement.h - ${H_PREFIX}/GyroscopeMeasurementDynamics.h + ${H_PREFIX}/GyroscopeMeasurementDynamics.h ${H_PREFIX}/RobotDynamicsEstimator.h PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler MANIF::manif BipedalLocomotion::System BipedalLocomotion::Math iDynTree::idyntree-high-level iDynTree::idyntree-core iDynTree::idyntree-model iDynTree::idyntree-modelio Eigen3::Eigen MANIF::manif BayesFilters::BayesFilters PRIVATE_LINK_LIBRARIES BipedalLocomotion::TextLogging BipedalLocomotion::ManifConversions diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h index 98dda46ee1..4f7beb2597 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h @@ -64,7 +64,7 @@ class UkfInputProvider : public System::Advanceable public: /** - * Get the inputes + * Get the input * @return A struct containing the inputs for the ukf */ const UKFInput& getOutput() const override; @@ -106,6 +106,7 @@ class Dynamics System::VariablesHandler m_stateVariableHandler; /**< Variable handler describing the variables and the sizes in the ukf state vector. */ bool m_isStateVariableHandlerSet{false}; /**< True if setVariableHandler is called. */ UKFInput m_ukfInput; + Eigen::VectorXd m_initialCovariances; /**< Vector of initial covariances. */ /** * Controls whether the variable handler contains the variables on which the dynamics depend. @@ -174,6 +175,12 @@ class Dynamics */ Eigen::Ref getCovariance(); + /** + * @brief getInitialStateCovariance access the covariance `Eigen::VectorXd` associated to the initial state. + * @return the vector of covariances. + */ + Eigen::Ref getInitialStateCovariance(); + /** * Destructor. */ diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h index e6629c43d0..102e48139f 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h @@ -76,6 +76,7 @@ class FrictionTorqueStateDynamics : public Dynamics * |:----------------------------------:|:--------:|:-------------------------------------------------------------------------------------------------------:|:---------:| * | `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 | diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h index 21f8a04f9d..40a295ba48 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h @@ -73,6 +73,7 @@ class JointVelocityStateDynamics : public Dynamics * |:----------------------------------:|:--------:|:-------------------------------------------------------------------------------------------------------:|:---------:| * | `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 | * | `dT` | `double` | Sampling time. | Yes | @@ -111,7 +112,6 @@ class JointVelocityStateDynamics : public Dynamics * @param ukfInput reference to the UKFInput struct. */ void setInput(const UKFInput & ukfInput) override; - }; BLF_REGISTER_DYNAMICS(JointVelocityStateDynamics, ::BipedalLocomotion::Estimators::RobotDynamicsEstimator::Dynamics); diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/RobotDynamicsEstimator.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/RobotDynamicsEstimator.h new file mode 100644 index 0000000000..ffdc6fb200 --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/RobotDynamicsEstimator.h @@ -0,0 +1,170 @@ +/** + * @file RobotDynamicsEstimator.h + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_ROBOT_DYNAMICS_ESTIMATOR_H +#define BIPEDAL_LOCOMOTION_ESTIMATORS_ROBOT_DYNAMICS_ESTIMATOR_H + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace Estimators +{ +namespace RobotDynamicsEstimator +{ + +/** + * Input of the RobotDynamicsEstimator containing both the inputs and the + * measurements for the Unscented Kalman Filter + */ +struct Input +{ + manif::SE3d basePose; /**< Pose describing the robot base position and orientation. */ + manif::SE3d::Tangent baseVelocity; /**< Velocity of the robot base. */ + manif::SE3d::Tangent baseAcceleration; /**< Mixed acceleration of the robot base. */ + Eigen::VectorXd jointPositions; /**< Joints positions in rad. */ + Eigen::VectorXd jointVelocities; /**< Joints velocities in rad per sec. */ + Eigen::VectorXd motorCurrents; /**< Motor currents in Ampere. */ + std::map ftWrenches; /**< Wrenches measured by force/torque sensors. */ + std::map linearAccelerations; /**< Linear accelerations measured by accelerometer sensors. */ + std::map angularVelocities; /**< Angular velocities measured by gyroscope sensors. */ +}; + +/** + * Output of the RobotDynamicsEstimator which represents the estimation of the + * Unscented Kalman Filter + */ +struct Output +{ + Eigen::VectorXd ds; /**< Joints velocities in rad per sec. */ + Eigen::VectorXd tau_m; /**< Motor toruqes in Nm. */ + Eigen::VectorXd tau_F; /**< Motor friction torques in Nm. */ + std::map ftWrenches; /**< Wrenches at the force/torque sensors. */ + std::map ftWrenchesBiases; /**< Biases of the force/torque sensors. */ + std::map accelerometerBiases; /**< Biases of the accelerometer sensors. */ + std::map gyroscopeBiases; /**< Biases of the gyroscope sensors. */ +}; + +/** + * RobotDynamicsEstimator is a concrete class and implements a robot dynamics estimator. + * The estimator is here implemented as an Unscented Kalman Filter. The user can build the estimator + * by using the build method or can initialize the RobotDynamicsEstimator object and then create the + * UkfState model and UkfMeasurement model which are then passed to the `bfl::UKFPrediction` and + * `bfl::UKFCorrection` objects respectively. To run an estimation step the user should set the + * input using `setInput`, call the `advance` method to perform an estimation step, use `getOutput` + * to get the estimation result. + */ +class RobotDynamicsEstimator : public System::Advanceable +{ + /** + * Private implementation + */ + struct Impl; + std::unique_ptr m_pimpl; + +public: + /** + * Constructor. + */ + RobotDynamicsEstimator(); + + /** + * Destructor. + */ + virtual ~RobotDynamicsEstimator(); + + /** + * Initialize the RobotDynamicsEstimator. + * @param handler pointer to the IParametersHandler + * @note + * | Group | Parameter Name | Type | Description | Mandatory | + * |:---------:|:------------------------------:|:---------------:|:----------------------------------------------------------------------------------------------:|:---------:| + * | `GENERAL` | `sampling_time` | `double` | Sampling time | Yes | + * | `UKF` | `alpha` | `double` | `alpha` parameter of the unscented kalman filter | Yes | + * | `UKF` | `beta` | `double` | `beta` parameter of the unscented kalman filter | Yes | + * | `UKF` | `kappa` | `double` | `kappa` parameter of the unscented kalman filter | Yes | + * @return True in case of success, false otherwise. + */ + bool initialize(std::weak_ptr handler) override; + + /** + * @brief Finalize the estimator. + * @param stateVariableHandler reference to a `System::VariablesHandler` object describing the ukf state. + * @param measurementVariableHandler reference to a `System::VariablesHandler` object describing the ukf measurement. + * @param kinDynFullModel a pointer to an iDynTree::KinDynComputations object. + * @note You should call this method after initialized the estimator and created the UkfState model + * which builds the handler needed in input to this method. + * @return true in case of success, false otherwise. + */ + bool finalize(const System::VariablesHandler& stateVariableHandler, + const System::VariablesHandler& measurementVariableHandler, + std::shared_ptr kinDynFullModel); + + /** + * @brief build a robot dynamics estimator implementing an unscented kalman filter. + * @param handler pointer to the IParametersHandler interface. + * @param kinDynFullModel a pointer to an iDynTree::KinDynComputations object that will be shared among + * all the dynamics. + * @param subModelList a list of SubModel objects + * @param kinDynWrapperList a list of pointers to a `SubModelKinDynWrapper` objects that will be shared among + * all the dynamics + * @return a RobotDynamicsEstimator. In case of issues an invalid RobotDynamicsEstimator + * will be returned. + */ + static std::unique_ptr + build(std::weak_ptr handler, + std::shared_ptr kinDynFullModel, + const std::vector& subModelList, + const std::vector>& kinDynWrapperList); + + /** + * @brief set the initial state of the estimator. + * @param initialState a reference to an `Output` object. + * @return true in case of success, false otherwise. + */ + bool setInitialState(const Output& initialState); + + /** + * @brief Determines the validity of the object retrieved with getOutput() + * @return True if the object is valid, false otherwise. + */ + bool isOutputValid() const override; + + /** + * @brief Advance the internal state. This may change the value retrievable from getOutput(). + * @return True if the advance is successfull. + */ + bool advance() override; + + /** + * Set the input for the estimator. + * @param input is a struct containing the input of the estimator. + */ + bool setInput(const Input& input) override; + + /** + * Get the output of the ukf + * @return A struct containing the ukf estimation result. + */ + const Output& getOutput() const override; + +}; // class RobotDynamicsEstimator + +} // namespace RobotDynamicsEstimator +} // namespace Estimators +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_ROBOT_DYNAMICS_ESTIMATOR_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h index f2692e34c0..a50cfabc5e 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h @@ -81,7 +81,7 @@ class SubModelKinDynWrapper * updateDynamicsVariableState updates the value of all the member variables containing * information about the robot kinematics and dynamics */ - bool updateDynamicsVariableState(); + bool updateDynamicsVariableState(bool isCorrectStep); public: /** @@ -105,7 +105,7 @@ class SubModelKinDynWrapper * @brief updateInternalKinDynState updates the state of the KinDynWrapper object. * @return a boolean value saying if the subModelList has been created correctly. */ - bool updateInternalKinDynState(); + bool updateInternalKinDynState(bool isCorrectStep); /** * @brief forwardDynamics computes the free floaing forward dynamics diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfMeasurement.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfMeasurement.h index 094cf653a2..79971b8bde 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfMeasurement.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfMeasurement.h @@ -157,7 +157,7 @@ class UkfMeasurement : public bfl::AdditiveMeasurementModel * @brief getMeasurementVariableHandler access the `System::VariablesHandler` instance created during the initialization phase. * @return the measurement variable handler containing all the measurement variables and their sizes and offsets. */ - System::VariablesHandler getMeasurementVariableHandler(); + System::VariablesHandler& getMeasurementVariableHandler(); /** * @brief predictedMeasure predict the new measurement depending on the state computed by the predict step. diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h index 435490b0d2..13d22722f9 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h @@ -153,7 +153,7 @@ class UkfState : public bfl::AdditiveStateModel * @brief getStateVariableHandler access the `System::VariablesHandler` instance created during the initialization phase. * @return the state variable handler containing all the state variables and their sizes and offsets. */ - System::VariablesHandler getStateVariableHandler(); + System::VariablesHandler& getStateVariableHandler(); /** * @brief propagate implements the predict of the ukf @@ -182,11 +182,17 @@ class UkfState : public bfl::AdditiveStateModel bfl::VectorDescription getStateDescription() override; /** - * @brief getStateSize access the length of the state vector - * @return the length of state vector + * @brief getStateSize access the length of the state vector. + * @return the length of state vector. */ std::size_t getStateSize(); + /** + * @brief getInitialStateCovarianceMatrix access the `Eigen::MatrixXd` representing the initial state covariance matrix. + * @return a Eigen reference to the Eigen Matrix covariance. + */ + Eigen::Ref getInitialStateCovarianceMatrix(); + }; // class UKFModel } // namespace RobotDynamicsEstimator diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h index 288d25b9f7..ef8b3e04f4 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h @@ -54,6 +54,7 @@ class ZeroVelocityDynamics : public Dynamics * |:----------------------------------:|:--------:|:-------------------------------------------------------------------------------------------------------:|:---------:| * | `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 | * | `use_bias` |`boolean` | Boolean saying if the dynamics depends on a bias. False if not specified. | No | diff --git a/src/Estimators/src/Dynamics.cpp b/src/Estimators/src/Dynamics.cpp index d751532a4e..e489d5715f 100644 --- a/src/Estimators/src/Dynamics.cpp +++ b/src/Estimators/src/Dynamics.cpp @@ -72,3 +72,8 @@ bool RDE::Dynamics::checkStateVariableHandler() { return true; } + +Eigen::Ref RDE::Dynamics::getInitialStateCovariance() +{ + return m_initialCovariances; +} diff --git a/src/Estimators/src/FrictionTorqueStateDynamics.cpp b/src/Estimators/src/FrictionTorqueStateDynamics.cpp index 6cd32a3ab6..29719404f3 100644 --- a/src/Estimators/src/FrictionTorqueStateDynamics.cpp +++ b/src/Estimators/src/FrictionTorqueStateDynamics.cpp @@ -41,6 +41,13 @@ bool RDE::FrictionTorqueStateDynamics::initialize(std::weak_ptrgetParameter("initial_covariance", m_initialCovariances)) + { + log()->error("{} Error while retrieving the initial_covariance variable.", errorPrefix); + return false; + } + // Set the dynamic model type if (!ptr->getParameter("dynamic_model", m_dynamicModel)) { @@ -251,6 +258,8 @@ bool RDE::FrictionTorqueStateDynamics::checkStateVariableHandler() return true; } +// TODO +// Change the model bool RDE::FrictionTorqueStateDynamics::update() { constexpr auto errorPrefix = "[FrictionTorqueStateDynamics::update]"; diff --git a/src/Estimators/src/JointVelocityStateDynamics.cpp b/src/Estimators/src/JointVelocityStateDynamics.cpp index 697c9a1681..7963695444 100644 --- a/src/Estimators/src/JointVelocityStateDynamics.cpp +++ b/src/Estimators/src/JointVelocityStateDynamics.cpp @@ -127,6 +127,13 @@ bool RDE::JointVelocityStateDynamics::initialize(std::weak_ptrgetParameter("initial_covariance", m_initialCovariances)) + { + log()->error("{} Error while retrieving the initial_covariance variable.", errorPrefix); + return false; + } + // Set the dynamic model type if (!ptr->getParameter("dynamic_model", m_dynamicModel)) { diff --git a/src/Estimators/src/RobotDynamicsEstimator.cpp b/src/Estimators/src/RobotDynamicsEstimator.cpp new file mode 100644 index 0000000000..74a92f8773 --- /dev/null +++ b/src/Estimators/src/RobotDynamicsEstimator.cpp @@ -0,0 +1,446 @@ +/** + * @file RobotDynamicsEstimator.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; +using namespace BipedalLocomotion; + +struct RobotDynamicsEstimator::Impl +{ + Output estimatorOutput; /**< Output of the estimator. */ + + UKFInput ukfInput; /**< Object to set the provider. */ + std::shared_ptr inputProvider; /**< Shared pointer used by all the dynamics. It needs to be updated here. */ + + std::map ukfMeasurementFromSensors; /**< This map containes the measurement coming from the sensors + and needed for the correction phase of the estimation. */ + + bfl::Gaussian predictedState; /**< Predicted state computed by the `predict` method. */ + bfl::Gaussian correctedState; /**< Corrected state computed by the `correct` method. */ + + double dT; /**< Sampling time. */ + double alpha, beta, kappa; /**< Parameters of the unscented kalman filter. */ + + std::unique_ptr stateModel; /**< UKF state model. */ + std::unique_ptr measurementModel; /**< UKF measurement model. */ + + std::unique_ptr ukfPrediction; + std::unique_ptr ukfCorrection; + + System::VariablesHandler stateHandler; /**< Handler of the ukf state. */ + System::VariablesHandler measurementHandler; /**< Handler of the ukf measurement. */ + + Eigen::MatrixXd initialStateCovariance; + + bool isInitialized{false}; + bool isFinalized{false}; + + bool isValid{false}; + + bool isInitialStateSet{false}; +}; + +RobotDynamicsEstimator::RobotDynamicsEstimator() +{ + m_pimpl = std::make_unique(); +} + +RobotDynamicsEstimator::~RobotDynamicsEstimator() = default; + +bool RobotDynamicsEstimator::initialize(std::weak_ptr handler) +{ + constexpr auto logPrefix = "[RobotDynamicsEstimator::initialize]"; + + auto ptr = handler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is not valid.", logPrefix); + return false; + } + + auto groupGeneral = ptr->getGroup("GENERAL").lock(); + if (groupGeneral == nullptr) + { + log()->error("{} Error while retrieving the group GENERAL.", logPrefix); + return false; + } + + if (!groupGeneral->getParameter("sampling_time", m_pimpl->dT)) + { + log()->error("{} Error while retrieving the sampling time variable.", logPrefix); + return false; + } + + auto groupUkf = ptr->getGroup("UKF").lock(); + if (groupUkf == nullptr) + { + log()->error("{} Error while retrieving the group UKF.", logPrefix); + return false; + } + + if (!groupUkf->getParameter("alpha", m_pimpl->alpha)) + { + log()->error("{} Error while retrieving the alpha variable.", logPrefix); + return false; + } + + if (!groupUkf->getParameter("beta", m_pimpl->beta)) + { + log()->error("{} Error while retrieving the beta variable.", logPrefix); + return false; + } + + if (!groupUkf->getParameter("kappa", m_pimpl->kappa)) + { + log()->error("{} Error while retrieving the kappa variable.", logPrefix); + return false; + } + + m_pimpl->inputProvider = std::make_shared(); + + m_pimpl->isInitialized = true; + + return true; +} + +bool RobotDynamicsEstimator::finalize(const System::VariablesHandler& stateVariableHandler, + const System::VariablesHandler& measurementVariableHandler, + std::shared_ptr kinDynFullModel) +{ + // Resize variables for the estimation + m_pimpl->predictedState.resize(stateVariableHandler.getNumberOfVariables()); + m_pimpl->predictedState.mean().setZero(); + m_pimpl->predictedState.covariance() = m_pimpl->initialStateCovariance; + + m_pimpl->correctedState.resize(stateVariableHandler.getNumberOfVariables()); + m_pimpl->correctedState.mean().setZero(); + m_pimpl->correctedState.covariance() = m_pimpl->initialStateCovariance; + + // Set the provider initial state + m_pimpl->ukfInput.robotBasePose.setIdentity(); + m_pimpl->ukfInput.robotBaseVelocity.setZero(); + m_pimpl->ukfInput.robotBaseAcceleration.setZero(); + m_pimpl->ukfInput.robotJointPositions.resize(kinDynFullModel->model().getNrOfDOFs()); + m_pimpl->inputProvider->setInput(m_pimpl->ukfInput); + + m_pimpl->isFinalized = true; + + return m_pimpl->isFinalized; +} + +std::unique_ptr RobotDynamicsEstimator::build(std::weak_ptr handler, + std::shared_ptr kinDynFullModel, + const std::vector& subModelList, + const std::vector>& kinDynWrapperList) +{ + constexpr auto logPrefix = "[RobotDynamicsEstimator::build]"; + + auto ptr = handler.lock(); + + if (ptr == nullptr) + { + log()->error("{} Invalid parameter handler.", logPrefix); + return nullptr; + } + + if (!kinDynFullModel->setFrameVelocityRepresentation(iDynTree::BODY_FIXED_REPRESENTATION)) + { + log()->error("{} Cannot set the frame velocity representation on the `iDynTree::KinDynComputation` " + "object describing the full robot model.", logPrefix); + return nullptr; + } + + std::unique_ptr estimator = std::make_unique(); + + if (!estimator->initialize(handler)) + { + log()->error("{} Error initializing the RobotDynamicsEstimator.", logPrefix); + return nullptr; + } + + auto groupUKF = ptr->getGroup("UKF").lock(); + if (groupUKF == nullptr) + { + log()->error("{} Error while retrieving the group UKF.", logPrefix); + return nullptr; + } + + auto groupUKFStateTmp = groupUKF->getGroup("UKF_STATE").lock(); + if (groupUKFStateTmp == nullptr) + { + log()->error("{} Error while retrieving the group UKF_STATE.", logPrefix); + return nullptr; + } + + auto groupUKFState = groupUKFStateTmp->clone(); + groupUKFState->setParameter("sampling_time", estimator->m_pimpl->dT); + + // Step 1 + // Create the state model for the ukf + estimator->m_pimpl->stateModel = UkfState::build(groupUKFState, + kinDynFullModel, + subModelList, + kinDynWrapperList); + if (estimator->m_pimpl->stateModel == nullptr) + { + log()->error("{} Error while creating the ukf state model.", logPrefix); + return nullptr; + } + + // Save a copy of the state variable handler + estimator->m_pimpl->stateHandler = estimator->m_pimpl->stateModel->getStateVariableHandler(); + + // Set the input provider + estimator->m_pimpl->stateModel->setUkfInputProvider(estimator->m_pimpl->inputProvider); + + // Get initial state covariance + estimator->m_pimpl->initialStateCovariance = estimator->m_pimpl->stateModel->getInitialStateCovarianceMatrix(); + + // Step 2 + // Initialize the unscented Kalman filter prediction step and pass the ownership of the state model + std::unique_ptr stateModelTemp = std::move(estimator->m_pimpl->stateModel); + estimator->m_pimpl->ukfPrediction = std::make_unique(std::move(stateModelTemp), + estimator->m_pimpl->alpha, + estimator->m_pimpl->beta, + estimator->m_pimpl->kappa); + + // Step 3 + // Create the measurement model for the ukf + auto groupUKFMeasTmp = groupUKF->getGroup("UKF_MEASUREMENT").lock(); + if (groupUKFMeasTmp == nullptr) + { + log()->error("{} Error while retrieving the group UKF_MEASUREMENT.", logPrefix); + return nullptr; + } + + auto groupUKFMeas = groupUKFMeasTmp->clone(); + groupUKFMeas->setParameter("sampling_time", estimator->m_pimpl->dT); + + // Create the measurement model for the ukf + estimator->m_pimpl->measurementModel = UkfMeasurement::build(groupUKFMeas, + estimator->m_pimpl->stateHandler, + kinDynFullModel, + subModelList, + kinDynWrapperList); + if (estimator->m_pimpl->measurementModel == nullptr) + { + log()->error("{} Error while creating the ukf measurement model.", logPrefix); + return nullptr; + } + + // Save a copy of the measurement variable handler + estimator->m_pimpl->measurementHandler = estimator->m_pimpl->measurementModel->getMeasurementVariableHandler(); + + // Set the input provider + estimator->m_pimpl->measurementModel->setUkfInputProvider(estimator->m_pimpl->inputProvider); + + // Step 4 + // Initialize the unscented Kalman filter correction step and pass the ownership of the measurement model. + std::unique_ptr measurementModelTemp = std::move(estimator->m_pimpl->measurementModel); + estimator->m_pimpl->ukfCorrection = std::make_unique(std::move(measurementModelTemp), + estimator->m_pimpl->alpha, + estimator->m_pimpl->beta, + estimator->m_pimpl->kappa); + + // Finalize the estimator + estimator->finalize(estimator->m_pimpl->stateHandler, estimator->m_pimpl->measurementHandler, kinDynFullModel); + + return estimator; +} + +bool RobotDynamicsEstimator::setInitialState(const Output& initialState) +{ + System::VariablesHandler::VariableDescription variable; + + if (m_pimpl->stateHandler.getVariable("ds", variable)) + { + m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = initialState.ds; + } + + if (m_pimpl->stateHandler.getVariable("tau_m", variable)) + { + m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = initialState.tau_m; + } + + if (m_pimpl->stateHandler.getVariable("tau_F", variable)) + { + 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)) + { + m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = val; + } + } + + for (auto const& [key, val] : initialState.ftWrenchesBiases) + { + if (m_pimpl->stateHandler.getVariable(key, variable)) + { + m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = val; + } + } + + for (auto const& [key, val] : initialState.gyroscopeBiases) + { + if (m_pimpl->stateHandler.getVariable(key, variable)) + { + m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = val; + } + } + + for (auto const& [key, val] : initialState.accelerometerBiases) + { + if (m_pimpl->stateHandler.getVariable(key, variable)) + { + m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = val; + } + } + + m_pimpl->estimatorOutput = initialState; + + m_pimpl->isInitialStateSet = true; + + return m_pimpl->isInitialStateSet; +} + +bool RobotDynamicsEstimator::isOutputValid() const +{ + return m_pimpl->isValid; +} + +bool RobotDynamicsEstimator::advance() +{ + constexpr auto logPrefix = "[RobotDynamicsEstimator::advance]"; + + // when advance is called the previous solution is no more valid + m_pimpl->isValid = false; + + if (!m_pimpl->isInitialized) + { + log()->error("{} Please call initialize() before advance().", logPrefix); + return false; + } + + if (!m_pimpl->isFinalized) + { + log()->error("{} Please call finalize() before advance().", logPrefix); + return false; + } + + if (!m_pimpl->isInitialStateSet) + { + log()->error("{} Please set the initial state before advance().", logPrefix); + return false; + } + + // Step 1 --> Predict + m_pimpl->ukfPrediction->predict(m_pimpl->correctedState, m_pimpl->predictedState); + + // Step 2 --> Set measurement + if (!m_pimpl->ukfCorrection->freeze_measurements(m_pimpl->ukfMeasurementFromSensors)) + { + log()->error("{} Cannot set the measurement to the UkfCorrection object.", logPrefix); + return false; + } + + // Step 3 --> Correct + m_pimpl->ukfCorrection->correct(m_pimpl->predictedState, m_pimpl->correctedState); + + m_pimpl->isValid = true; + + return true; +} + +bool RobotDynamicsEstimator::setInput(const Input & input) +{ + constexpr auto logPrefix = "[RobotDynamicsEstimator::setInput]"; + + // Set the input provider state + m_pimpl->ukfInput.robotBasePose = input.basePose; + m_pimpl->ukfInput.robotBaseVelocity = input.baseVelocity; + m_pimpl->ukfInput.robotBaseAcceleration = input.baseAcceleration; + m_pimpl->ukfInput.robotJointPositions = input.jointPositions; + + if (!m_pimpl->inputProvider->setInput(m_pimpl->ukfInput)) + { + log()->error("{} Cannot set the input of the input provider.", logPrefix); + return false; + } + + // Set the `std::map` used as measurement object + // for the freeze method of the UkfCorrection + m_pimpl->ukfMeasurementFromSensors["ds"] = input.jointVelocities; + m_pimpl->ukfMeasurementFromSensors["i_m"] = input.motorCurrents; + for (auto & [key, value] : input.ftWrenches) + { + m_pimpl->ukfMeasurementFromSensors[key] = value; + } + for (auto & [key, value] : input.linearAccelerations) + { + m_pimpl->ukfMeasurementFromSensors[key] = value; + } + for (auto & [key, value] : input.angularVelocities) + { + m_pimpl->ukfMeasurementFromSensors[key] = value; + } + + return true; +} + +const Output& RobotDynamicsEstimator::getOutput() const +{ + if (m_pimpl->isInitialStateSet) + { + m_pimpl->estimatorOutput.ds = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable("ds").offset, + m_pimpl->stateHandler.getVariable("ds").size); + + m_pimpl->estimatorOutput.tau_m = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable("tau_m").offset, + m_pimpl->stateHandler.getVariable("tau_m").size); + + m_pimpl->estimatorOutput.tau_F = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable("tau_F").offset, + m_pimpl->stateHandler.getVariable("tau_F").size); + + for (auto & [key, value] : m_pimpl->estimatorOutput.ftWrenches) + { + m_pimpl->estimatorOutput.ftWrenches[key] = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable(key).offset, + m_pimpl->stateHandler.getVariable(key).size); + } + + for (auto & [key, value] : m_pimpl->estimatorOutput.ftWrenchesBiases) + { + m_pimpl->estimatorOutput.ftWrenchesBiases[key] = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable(key).offset, + m_pimpl->stateHandler.getVariable(key).size); + } + + for (auto & [key, value] : m_pimpl->estimatorOutput.accelerometerBiases) + { + m_pimpl->estimatorOutput.accelerometerBiases[key] = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable(key).offset, + m_pimpl->stateHandler.getVariable(key).size); + } + + for (auto & [key, value] : m_pimpl->estimatorOutput.gyroscopeBiases) + { + m_pimpl->estimatorOutput.gyroscopeBiases[key] = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable(key).offset, + m_pimpl->stateHandler.getVariable(key).size); + } + } + + return m_pimpl->estimatorOutput; +} diff --git a/src/Estimators/src/SubModel.cpp b/src/Estimators/src/SubModel.cpp index 889db6d367..18f3c37c53 100644 --- a/src/Estimators/src/SubModel.cpp +++ b/src/Estimators/src/SubModel.cpp @@ -544,7 +544,6 @@ const RDE::Sensor& RDE::SubModel::getAccelerometer(const std::string name) const bool RDE::SubModel::hasAccelerometer(const std::string name) const { - std::cout << "Name --> " << name << std::endl; for (int accIndex = 0; accIndex < m_accelerometerList.size(); accIndex++) { if (m_accelerometerList[accIndex].name == name) diff --git a/src/Estimators/src/SubModelDynamics.cpp b/src/Estimators/src/SubModelDynamics.cpp index d25deffea3..adbbe55fb3 100644 --- a/src/Estimators/src/SubModelDynamics.cpp +++ b/src/Estimators/src/SubModelDynamics.cpp @@ -6,6 +6,8 @@ */ #include +#include +#include #include #include @@ -105,15 +107,7 @@ void RDE::SubModelDynamics::setState(const Eigen::Ref ukf for (int idx = 0; idx < subModel.getModel().getNrOfDOFs(); idx++) { jointVelocity(idx) = jointVelocityFullModel(subModel.getJointMapping().at(idx)); - } - - for (int idx = 0; idx < subModel.getModel().getNrOfDOFs(); idx++) - { motorTorque(idx) = motorTorqueFullModel(subModel.getJointMapping().at(idx)); - } - - for (int idx = 0; idx < subModel.getModel().getNrOfDOFs(); idx++) - { frictionTorque(idx) = frictionTorqueFullModel(subModel.getJointMapping().at(idx)); } diff --git a/src/Estimators/src/SubModelKinDynWrapper.cpp b/src/Estimators/src/SubModelKinDynWrapper.cpp index 53560dbafb..9cc2c08967 100644 --- a/src/Estimators/src/SubModelKinDynWrapper.cpp +++ b/src/Estimators/src/SubModelKinDynWrapper.cpp @@ -116,7 +116,7 @@ bool RDE::SubModelKinDynWrapper::initialize(const RDE::SubModel& subModel) return true; } -bool RDE::SubModelKinDynWrapper::updateInternalKinDynState() +bool RDE::SubModelKinDynWrapper::updateInternalKinDynState(bool isCorrectStep) { constexpr auto logPrefix = "[BipedalLocomotion::Estimators::SubModelKinDynWrapper::" "updateKinDynState]"; @@ -148,10 +148,10 @@ bool RDE::SubModelKinDynWrapper::updateInternalKinDynState() return false; } - return updateDynamicsVariableState(); + return updateDynamicsVariableState(isCorrectStep); } -bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState() +bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState(bool isCorrectStep) { constexpr auto logPrefix = "[BipedalLocomotion::Estimators::SubModelKinDynWrapper::" "updateDynamicsVariableState]"; @@ -185,45 +185,48 @@ bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState() } } - // Update accelerometer jacobians, dJnu, rotMatrix - for (int idx = 0; idx < m_subModel.getAccelerometerList().size(); idx++) + if (isCorrectStep) { - // Update jacobian - if (!m_kinDyn - .getFrameFreeFloatingJacobian(m_subModel.getAccelerometerList()[idx].frame, - m_jacAccList[m_subModel.getAccelerometerList()[idx] - .name])) + // Update accelerometer jacobians, dJnu, rotMatrix + for (int idx = 0; idx < m_subModel.getAccelerometerList().size(); idx++) { - blf::log()->error("{} Unable to get the compute the free floating jacobian of the " - "frame {}.", - logPrefix, - m_subModel.getAccelerometerList()[idx].frame); - return false; + // Update jacobian + if (!m_kinDyn + .getFrameFreeFloatingJacobian(m_subModel.getAccelerometerList()[idx].frame, + m_jacAccList[m_subModel.getAccelerometerList()[idx] + .name])) + { + blf::log()->error("{} Unable to get the compute the free floating jacobian of the " + "frame {}.", + logPrefix, + m_subModel.getAccelerometerList()[idx].frame); + return false; + } + + // Update dJnu + m_dJnuList[m_subModel.getAccelerometerList()[idx].name] = iDynTree::toEigen( + m_kinDyn.getFrameBiasAcc(m_subModel.getAccelerometerList()[idx].frame)); + + // Update rotMatrix + m_accRworldList[m_subModel.getAccelerometerList()[idx].name] = blf::Conversions::toManifRot( + m_kinDyn.getWorldTransform(m_subModel.getAccelerometerList()[idx].frame) + .getRotation() + .inverse()); } - // Update dJnu - m_dJnuList[m_subModel.getAccelerometerList()[idx].name] = iDynTree::toEigen( - m_kinDyn.getFrameBiasAcc(m_subModel.getAccelerometerList()[idx].frame)); - - // Update rotMatrix - m_accRworldList[m_subModel.getAccelerometerList()[idx].name] = blf::Conversions::toManifRot( - m_kinDyn.getWorldTransform(m_subModel.getAccelerometerList()[idx].frame) - .getRotation() - .inverse()); - } - - // Update gyroscope jacobians - for (int idx = 0; idx < m_subModel.getGyroscopeList().size(); idx++) - { - if (!m_kinDyn.getFrameFreeFloatingJacobian(m_subModel.getGyroscopeList()[idx].frame, - m_jacGyroList[m_subModel.getGyroscopeList()[idx] - .name])) + // Update gyroscope jacobians + for (int idx = 0; idx < m_subModel.getGyroscopeList().size(); idx++) { - blf::log()->error("{} Unable to get the compute the free floating jacobian of the " - "frame {}.", - logPrefix, - m_subModel.getGyroscopeList()[idx].frame); - return false; + if (!m_kinDyn.getFrameFreeFloatingJacobian(m_subModel.getGyroscopeList()[idx].frame, + m_jacGyroList[m_subModel.getGyroscopeList()[idx] + .name])) + { + blf::log()->error("{} Unable to get the compute the free floating jacobian of the " + "frame {}.", + logPrefix, + m_subModel.getGyroscopeList()[idx].frame); + return false; + } } } @@ -277,7 +280,7 @@ bool RDE::SubModelKinDynWrapper::forwardDynamics(Eigen::Ref mot m_FTBaseAcc = m_FTranspose * baseAcceleration; m_totalTorques - = motorTorqueAfterGearbox - frictionTorques - m_genBiasJointTorques + tauExt + m_FTBaseAcc; + = motorTorqueAfterGearbox - frictionTorques - m_genBiasJointTorques + tauExt - m_FTBaseAcc; jointAcceleration = m_pseudoInverseH * m_totalTorques; diff --git a/src/Estimators/src/UkfMeasurement.cpp b/src/Estimators/src/UkfMeasurement.cpp index d7c1de9969..c3b15b75e8 100644 --- a/src/Estimators/src/UkfMeasurement.cpp +++ b/src/Estimators/src/UkfMeasurement.cpp @@ -17,13 +17,15 @@ namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; +using namespace std::chrono; + struct RDE::UkfMeasurement::Impl { bool isInitialized{false}; bool isFinalized{false}; - bfl::VectorDescription measurementDescription_; - bfl::VectorDescription inputDescription_; + bfl::VectorDescription measurementDescription; + bfl::VectorDescription inputDescription; Eigen::Vector3d gravity{0, 0, -Math::StandardAccelerationOfGravitation}; /**< Gravity vector. */ @@ -46,7 +48,8 @@ struct RDE::UkfMeasurement::Impl Eigen::VectorXd jointVelocityState; /**< Joint velocity compute by the ukf. */ Eigen::VectorXd currentState; /**< State estimated in the previous step. */ - Eigen::VectorXd predictedMeasurement; /**< Vector containing the updated measurement. */ + Eigen::VectorXd tempPredictedMeas; + Eigen::MatrixXd predictedMeasurement; /**< Vector containing the updated measurement. */ Eigen::VectorXd measurement; /**< Measurements coming from the sensors. */ @@ -126,12 +129,13 @@ bool RDE::UkfMeasurement::finalize(const System::VariablesHandler& handler) m_pimpl->currentState.resize(m_pimpl->measurementSize); m_pimpl->currentState.setZero(); - m_pimpl->predictedMeasurement.resize(m_pimpl->measurementSize); - m_pimpl->predictedMeasurement.setZero(); - m_pimpl->measurement.resize(m_pimpl->measurementSize); m_pimpl->measurement.setZero(); + m_pimpl->tempPredictedMeas.resize(m_pimpl->measurementSize); + + m_pimpl->measurementDescription = bfl::VectorDescription(m_pimpl->measurementSize); + m_pimpl->isFinalized = true; return true; @@ -243,6 +247,8 @@ std::unique_ptr RDE::UkfMeasurement::build(std::weak_ptrm_pimpl->dynamicsList.insert({dynamicsName, dynamicsInstance}); } + measurement->m_pimpl->inputDescription = bfl::VectorDescription(stateVariableHandler.getNumberOfVariables()); + // finalize estimator if (!measurement->finalize(measurement->m_pimpl->stateVariableHandler)) { @@ -265,18 +271,23 @@ std::pair RDE::UkfMeasurement::getNoiseCovarianceMatrix() bfl::VectorDescription RDE::UkfMeasurement::getMeasurementDescription() const { - return bfl::VectorDescription(m_pimpl->measurementSize); + return m_pimpl->measurementDescription; } bfl::VectorDescription RDE::UkfMeasurement::getInputDescription() const { - return bfl::VectorDescription(m_pimpl->currentState.size(), 0, m_pimpl->measurementSize); + return m_pimpl->inputDescription; } +// TODO +// Here the cur_state has size state_size x n_sigma_points +// this means that the computation can be parallelized std::pair RDE::UkfMeasurement::predictedMeasure(const Eigen::Ref& cur_states) const { constexpr auto logPrefix = "[UkfMeasurement::propagate]"; + m_pimpl->predictedMeasurement.resize(m_pimpl->measurementSize, cur_states.cols()); + // Check that everything is initialized and set if (!m_pimpl->isFinalized) { @@ -293,51 +304,55 @@ std::pair RDE::UkfMeasurement::predictedMeasure(const Eigen::Re // Get input of ukf from provider m_pimpl->ukfInput = m_pimpl->ukfInputProvider->getOutput(); - m_pimpl->currentState = cur_states; + for (int index = 0; index < cur_states.cols(); index++) + { + m_pimpl->currentState = cur_states.block(0, index, cur_states.rows(), 1);; - m_pimpl->jointVelocityState = m_pimpl->currentState.segment(m_pimpl->stateVariableHandler.getVariable("ds").offset, - m_pimpl->stateVariableHandler.getVariable("ds").size); + m_pimpl->jointVelocityState = m_pimpl->currentState.segment(m_pimpl->stateVariableHandler.getVariable("ds").offset, + m_pimpl->stateVariableHandler.getVariable("ds").size); - // Update kindyn full model - m_pimpl->kinDynFullModel->setRobotState(m_pimpl->ukfInput.robotBasePose.transform(), - m_pimpl->ukfInput.robotJointPositions, - iDynTree::make_span(m_pimpl->ukfInput.robotBaseVelocity.data(), manif::SE3d::Tangent::DoF), - m_pimpl->jointVelocityState, - m_pimpl->gravity); + // Update kindyn full model + m_pimpl->kinDynFullModel->setRobotState(m_pimpl->ukfInput.robotBasePose.transform(), + m_pimpl->ukfInput.robotJointPositions, + iDynTree::make_span(m_pimpl->ukfInput.robotBaseVelocity.data(), manif::SE3d::Tangent::DoF), + m_pimpl->jointVelocityState, + m_pimpl->gravity); - // Update kindyn sub-models - for (int subModelIdx = 0; subModelIdx < m_pimpl->subModelList.size(); subModelIdx++) - { - m_pimpl->kinDynWrapperList[subModelIdx]->updateInternalKinDynState(); - } + // Update kindyn sub-models + for (int subModelIdx = 0; subModelIdx < m_pimpl->subModelList.size(); subModelIdx++) + { + m_pimpl->kinDynWrapperList[subModelIdx]->updateInternalKinDynState(true); + } - // TODO - // This could be parallelized + // TODO + // This could be parallelized - // Update all the dynamics - for (auto& [name, dynamics] : m_pimpl->dynamicsList) - { - dynamics->setState(m_pimpl->currentState); + // Update all the dynamics + for (auto& [name, dynamics] : m_pimpl->dynamicsList) + { + dynamics->setState(m_pimpl->currentState); - dynamics->setInput(m_pimpl->ukfInput); + dynamics->setInput(m_pimpl->ukfInput); - if (!dynamics->update()) - { - BipedalLocomotion::log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, name); - throw std::runtime_error("Error"); + if (!dynamics->update()) + { + BipedalLocomotion::log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, name); + throw std::runtime_error("Error"); + } + + m_pimpl->tempPredictedMeas.segment(m_pimpl->measurementVariableHandler.getVariable(name).offset, + m_pimpl->measurementVariableHandler.getVariable(name).size) = dynamics->getUpdatedVariable(); } + m_pimpl->predictedMeasurement.block(0, index, m_pimpl->measurementSize, 1) = m_pimpl->tempPredictedMeas; - m_pimpl->predictedMeasurement.segment(m_pimpl->measurementVariableHandler.getVariable(name).offset, - m_pimpl->measurementVariableHandler.getVariable(name).size) = dynamics->getUpdatedVariable(); } - return std::make_pair(true, std::move(m_pimpl->predictedMeasurement)); + return std::make_pair(true, m_pimpl->predictedMeasurement); } std::pair RDE::UkfMeasurement::innovation(const bfl::Data& predicted_measurements, const bfl::Data& measurements) const { - Eigen::MatrixXd innovation = -(bfl::any::any_cast(predicted_measurements).colwise() - - bfl::any::any_cast(measurements).col(0)); + Eigen::MatrixXd innovation = -(bfl::any::any_cast(predicted_measurements).colwise() - bfl::any::any_cast(measurements)); return std::make_pair(true, std::move(innovation)); } @@ -347,7 +362,7 @@ std::size_t RDE::UkfMeasurement::getMeasurementSize() return m_pimpl->measurementSize; } -BipedalLocomotion::System::VariablesHandler RDE::UkfMeasurement::getMeasurementVariableHandler() +BipedalLocomotion::System::VariablesHandler& RDE::UkfMeasurement::getMeasurementVariableHandler() { return m_pimpl->measurementVariableHandler; } @@ -370,6 +385,8 @@ bool RDE::UkfMeasurement::freeze(const bfl::Data& data) { m_pimpl->offsetMeasurement = m_pimpl->measurementVariableHandler.getVariable(name).offset; + // 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_pimpl->offsetMeasurement < (m_pimpl->measurementVariableHandler.getVariable(name).offset + m_pimpl->measurementVariableHandler.getVariable(name).size)) { m_pimpl->measurement.segment(m_pimpl->offsetMeasurement, m_pimpl->measurementMap[name].size()) diff --git a/src/Estimators/src/UkfState.cpp b/src/Estimators/src/UkfState.cpp index 4940d5f2a4..c705429739 100644 --- a/src/Estimators/src/UkfState.cpp +++ b/src/Estimators/src/UkfState.cpp @@ -6,6 +6,7 @@ */ #include +#include #include #include @@ -20,6 +21,8 @@ using namespace BipedalLocomotion; namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; +using namespace std::chrono; + struct RDE::UkfState::Impl { bool isInitialized{false}; @@ -28,6 +31,7 @@ struct RDE::UkfState::Impl Eigen::Vector3d gravity{0, 0, -Math::StandardAccelerationOfGravitation}; /**< Gravity vector. */ Eigen::MatrixXd covarianceQ; /**< Covariance matrix. */ + Eigen::MatrixXd initialCovariance; /**< Initial covariance matrix. */ std::size_t stateSize; /**< Length of the state vector. */ double dT; /**< Sampling time */ @@ -107,10 +111,17 @@ bool RDE::UkfState::finalize(const System::VariablesHandler& handler) m_pimpl->covarianceQ.resize(m_pimpl->stateSize, m_pimpl->stateSize); m_pimpl->covarianceQ.setZero(); + m_pimpl->initialCovariance.resize(m_pimpl->stateSize, m_pimpl->stateSize); + m_pimpl->initialCovariance.setZero(); + for (auto& [name, dynamics] : m_pimpl->dynamicsList) { m_pimpl->covarianceQ.block(handler.getVariable(name).offset, handler.getVariable(name).offset, handler.getVariable(name).size, handler.getVariable(name).size) = dynamics->getCovariance().asDiagonal(); + + m_pimpl->initialCovariance.block(handler.getVariable(name).offset, handler.getVariable(name).offset, + handler.getVariable(name).size, handler.getVariable(name).size) = dynamics->getInitialStateCovariance().asDiagonal(); + } m_pimpl->jointVelocityState.resize(m_pimpl->kinDynFullModel->model().getNrOfDOFs()); @@ -201,6 +212,11 @@ std::unique_ptr RDE::UkfState::build(std::weak_ptrerror("{} Unable to find the parameter 'covariance'.", logPrefix); return nullptr; } + if (!dynamicsGroup->getParameter("initial_covariance", covariances)) + { + log()->error("{} Unable to find the parameter 'initial_covariance'.", logPrefix); + return nullptr; + } if (!dynamicsGroup->getParameter("elements", dynamicsElements)) { log()->error("{} Unable to find the parameter 'elements'.", logPrefix); @@ -250,11 +266,14 @@ Eigen::MatrixXd RDE::UkfState::getNoiseCovarianceMatrix() return m_pimpl->covarianceQ; } -System::VariablesHandler RDE::UkfState::getStateVariableHandler() +System::VariablesHandler& RDE::UkfState::getStateVariableHandler() { return m_pimpl->stateVariableHandler; } +// TODO +// Here the cur_state has size state_size x n_sigma_points +// this means that the computation can be parallelized void RDE::UkfState::propagate(const Eigen::Ref& cur_states, Eigen::Ref prop_states) { constexpr auto logPrefix = "[UkfState::propagate]"; @@ -275,45 +294,50 @@ void RDE::UkfState::propagate(const Eigen::Ref& cur_state // Get input of ukf from provider m_pimpl->ukfInput = m_pimpl->ukfInputProvider->getOutput(); - m_pimpl->currentState = cur_states; - - m_pimpl->jointVelocityState = m_pimpl->currentState.segment(m_pimpl->stateVariableHandler.getVariable("ds").offset, - m_pimpl->stateVariableHandler.getVariable("ds").size); - - // Update kindyn full model - m_pimpl->kinDynFullModel->setRobotState(m_pimpl->ukfInput.robotBasePose.transform(), - m_pimpl->ukfInput.robotJointPositions, - iDynTree::make_span(m_pimpl->ukfInput.robotBaseVelocity.data(), manif::SE3d::Tangent::DoF), - m_pimpl->jointVelocityState, - m_pimpl->gravity); + prop_states.resize(cur_states.rows(), cur_states.cols()); - // Update kindyn sub-models - for (int subModelIdx = 0; subModelIdx < m_pimpl->subModelList.size(); subModelIdx++) + for (int index = 0; index < cur_states.cols(); index++) { - m_pimpl->kinDynWrapperList[subModelIdx]->updateInternalKinDynState(); - } + m_pimpl->currentState = cur_states.block(0, index, cur_states.rows(), 1); - // TODO - // This could be parallelized + m_pimpl->jointVelocityState = m_pimpl->currentState.segment(m_pimpl->stateVariableHandler.getVariable("ds").offset, + m_pimpl->stateVariableHandler.getVariable("ds").size); - // Update all the dynamics - for (auto& [name, dynamics] : m_pimpl->dynamicsList) - { - dynamics->setState(m_pimpl->currentState); + // Update kindyn full model + m_pimpl->kinDynFullModel->setRobotState(m_pimpl->ukfInput.robotBasePose.transform(), + m_pimpl->ukfInput.robotJointPositions, + iDynTree::make_span(m_pimpl->ukfInput.robotBaseVelocity.data(), manif::SE3d::Tangent::DoF), + m_pimpl->jointVelocityState, + m_pimpl->gravity); + + // Update kindyn sub-models + for (int subModelIdx = 0; subModelIdx < m_pimpl->subModelList.size(); subModelIdx++) + { + m_pimpl->kinDynWrapperList[subModelIdx]->updateInternalKinDynState(false); + } - dynamics->setInput(m_pimpl->ukfInput); + // TODO + // This could be parallelized - if (!dynamics->update()) + // Update all the dynamics + for (auto& [name, dynamics] : m_pimpl->dynamicsList) { - log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, name); - throw std::runtime_error("Error"); + dynamics->setState(m_pimpl->currentState); + + dynamics->setInput(m_pimpl->ukfInput); + + if (!dynamics->update()) + { + log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, name); + throw std::runtime_error("Error"); + } + + m_pimpl->nextState.segment(m_pimpl->stateVariableHandler.getVariable(name).offset, + m_pimpl->stateVariableHandler.getVariable(name).size) = dynamics->getUpdatedVariable(); } - m_pimpl->nextState.segment(m_pimpl->stateVariableHandler.getVariable(name).offset, - m_pimpl->stateVariableHandler.getVariable(name).size) = dynamics->getUpdatedVariable(); + prop_states.block(0, index, cur_states.rows(), 1) = m_pimpl->nextState; } - - prop_states = m_pimpl->nextState; } bfl::VectorDescription RDE::UkfState::getStateDescription() @@ -325,3 +349,8 @@ std::size_t RDE::UkfState::getStateSize() { return m_pimpl->stateSize; } + +Eigen::Ref RDE::UkfState::getInitialStateCovarianceMatrix() +{ + return m_pimpl->initialCovariance; +} diff --git a/src/Estimators/src/ZeroVelocityDynamics.cpp b/src/Estimators/src/ZeroVelocityDynamics.cpp index 6a2a6c4431..764ab8a830 100644 --- a/src/Estimators/src/ZeroVelocityDynamics.cpp +++ b/src/Estimators/src/ZeroVelocityDynamics.cpp @@ -35,6 +35,12 @@ bool RDE::ZeroVelocityDynamics::initialize(std::weak_ptrgetParameter("initial_covariance", m_initialCovariances)) + { + log()->debug("{} Variable initial_covariance not found.", errorPrefix); + } + // Set the dynamic model type if (!ptr->getParameter("dynamic_model", m_dynamicModel)) { @@ -45,14 +51,14 @@ bool RDE::ZeroVelocityDynamics::initialize(std::weak_ptrgetParameter("elements", m_elements)) { - log()->info("{} Variable elements not found.", errorPrefix); + log()->debug("{} Variable elements not found.", errorPrefix); m_elements = {}; } // Set the bias related variables if use_bias is true if (!ptr->getParameter("use_bias", m_useBias)) { - log()->info("{} Variable use_bias not found. Set to false by default.", errorPrefix); + log()->debug("{} Variable use_bias not found. Set to false by default.", errorPrefix); } else { diff --git a/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp index ea22d24c68..341d540c40 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp @@ -160,7 +160,7 @@ TEST_CASE("Friction Torque Dynamics") kinDynWrapperList.emplace_back(std::make_shared()); REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); - REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState()); + REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState(true)); } AccelerometerMeasurementDynamics accDynamics; diff --git a/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt b/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt index f3ddf4d7b4..a783e50060 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt +++ b/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt @@ -20,8 +20,7 @@ if(FRAMEWORK_USE_icub-models AND FRAMEWORK_COMPILE_YarpImplementation) BipedalLocomotion::Math icub-models::icub-models Eigen3::Eigen - MANIF::manif - ) + MANIF::manif) add_bipedal_test(NAME ZeroVelocityDynamics SOURCES ZeroVelocityDynamicsTest.cpp @@ -29,8 +28,7 @@ if(FRAMEWORK_USE_icub-models AND FRAMEWORK_COMPILE_YarpImplementation) BipedalLocomotion::ParametersHandler BipedalLocomotion::ParametersHandlerYarpImplementation icub-models::icub-models - Eigen3::Eigen - ) + Eigen3::Eigen) add_bipedal_test(NAME FrictionTorqueStateDynamics SOURCES FrictionTorqueStateDynamicsTest.cpp @@ -39,8 +37,7 @@ if(FRAMEWORK_USE_icub-models AND FRAMEWORK_COMPILE_YarpImplementation) BipedalLocomotion::ParametersHandlerYarpImplementation BipedalLocomotion::System icub-models::icub-models - Eigen3::Eigen - ) + Eigen3::Eigen) add_bipedal_test(NAME JointVelocityStateDynamics SOURCES JointVelocityStateDynamicsTest.cpp @@ -50,8 +47,7 @@ if(FRAMEWORK_USE_icub-models AND FRAMEWORK_COMPILE_YarpImplementation) BipedalLocomotion::System icub-models::icub-models Eigen3::Eigen - MANIF::manif - ) + MANIF::manif) add_bipedal_test(NAME AccelerometerMeasurementDynamics SOURCES AccelerometerMeasurementDynamicsTest.cpp @@ -61,8 +57,7 @@ if(FRAMEWORK_USE_icub-models AND FRAMEWORK_COMPILE_YarpImplementation) BipedalLocomotion::System icub-models::icub-models Eigen3::Eigen - MANIF::manif - ) + MANIF::manif) add_bipedal_test(NAME GyroscopeMeasurementDynamics SOURCES GyroscopeMeasurementDynamicsTest.cpp @@ -72,20 +67,22 @@ if(FRAMEWORK_USE_icub-models AND FRAMEWORK_COMPILE_YarpImplementation) BipedalLocomotion::System icub-models::icub-models Eigen3::Eigen - MANIF::manif - ) + MANIF::manif) add_bipedal_test(NAME UkfState SOURCES UkfStateTest.cpp LINKS BipedalLocomotion::RobotDynamicsEstimator BipedalLocomotion::ManifConversions BipedalLocomotion::ParametersHandlerYarpImplementation - icub-models::icub-models - ) + icub-models::icub-models) add_bipedal_test(NAME UkfMeasurement SOURCES UkfMeasurementTest.cpp LINKS BipedalLocomotion::RobotDynamicsEstimator BipedalLocomotion::ManifConversions BipedalLocomotion::ParametersHandlerYarpImplementation - icub-models::icub-models - ) + icub-models::icub-models) + + add_bipedal_test(NAME RobotDynamicsEstimator + SOURCES RobotDynamicsEstimatorTest.cpp + LINKS BipedalLocomotion::RobotDynamicsEstimator BipedalLocomotion::ManifConversions BipedalLocomotion::ParametersHandlerYarpImplementation + icub-models::icub-models) include_directories(${CMAKE_CURRENT_BINARY_DIR}) configure_file("${CMAKE_CURRENT_SOURCE_DIR}/ConfigFolderPath.h.in" "${CMAKE_CURRENT_BINARY_DIR}/ConfigFolderPath.h" @ONLY) diff --git a/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp index 2cee47a5b0..bec23114e3 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp @@ -108,6 +108,7 @@ TEST_CASE("Friction Torque Dynamics") parameterHandler->setParameter("name", name); parameterHandler->setParameter("covariance", covariance); + parameterHandler->setParameter("initial_covariance", covariance); parameterHandler->setParameter("dynamic_model", model); parameterHandler->setParameter("elements", elements); parameterHandler->setParameter("friction_k0", k0); @@ -155,7 +156,7 @@ TEST_CASE("Friction Torque Dynamics") kinDynWrapperList.emplace_back(std::make_shared()); REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); - REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState()); + REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState(true)); } FrictionTorqueStateDynamics tau_F_dynamics; @@ -209,6 +210,29 @@ TEST_CASE("Friction Torque Dynamics") state.segment(variableHandler.getVariable("tau_F").offset, variableHandler.getVariable("tau_F").size) = currentStateTauF; + // Create an input for the ukf state + UKFInput input; + + // Define joint positions + Eigen::VectorXd jointPos; + jointPos.resize(kinDyn->model().getNrOfDOFs()); + jointPos.setZero(); + input.robotJointPositions = jointPos; + + // Define base pose + manif::SE3d basePose; + basePose.setIdentity(); + input.robotBasePose = basePose; + + // Define base velocity and acceleration + manif::SE3d::Tangent baseVelocity, baseAcceleration; + baseVelocity.setZero(); + baseAcceleration.setZero(); + input.robotBaseVelocity = baseVelocity; + input.robotBaseAcceleration = baseAcceleration; + + tau_F_dynamics.setInput(input); + tau_F_dynamics.setState(state); REQUIRE(tau_F_dynamics.update()); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp index 07d4c351bf..4b01f3a10a 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp @@ -160,7 +160,7 @@ TEST_CASE("Gyroscope Measurement Dynamics") kinDynWrapperList.emplace_back(std::make_shared()); REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); - REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState()); + REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState(true)); } GyroscopeMeasurementDynamics gyroDynamics; @@ -197,6 +197,29 @@ TEST_CASE("Gyroscope Measurement Dynamics") state[offset+jointIndex] = jointTorques.jointTorques()[jointIndex]; } + // Create an input for the ukf state + UKFInput input; + + // Define joint positions + Eigen::VectorXd jointPos; + jointPos.resize(kinDyn->model().getNrOfDOFs()); + jointPos.setZero(); + input.robotJointPositions = jointPos; + + // Define base pose + manif::SE3d basePose; + basePose.setIdentity(); + input.robotBasePose = basePose; + + // Define base velocity and acceleration + manif::SE3d::Tangent baseVelocity, baseAcceleration; + baseVelocity.setZero(); + baseAcceleration.setZero(); + input.robotBaseVelocity = baseVelocity; + input.robotBaseAcceleration = baseAcceleration; + + gyroDynamics.setInput(input); + gyroDynamics.setState(state); REQUIRE(gyroDynamics.update()); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp index 28220ce965..6d0937f817 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp @@ -58,7 +58,8 @@ IParametersHandler::shared_ptr loadParameterHandler() } void createModelLoader(IParametersHandler::shared_ptr group, - iDynTree::ModelLoader& mdlLdr) + iDynTree::ModelLoader& mdlLdr, + bool useFT) { // List of joints and fts to load the model std::vector subModelList; @@ -68,13 +69,16 @@ void createModelLoader(IParametersHandler::shared_ptr group, std::vector jointList; REQUIRE(group->getParameter("joint_list", jointList)); - std::vector ftFramesList; - auto ftGroup = group->getGroup("FT").lock(); - REQUIRE(ftGroup->getParameter("frames", ftFramesList)); - std::vector jointsAndFTs; jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); - jointsAndFTs.insert(jointsAndFTs.end(), ftFramesList.begin(), ftFramesList.end()); + + if (useFT) + { + std::vector ftFramesList; + auto ftGroup = group->getGroup("FT").lock(); + REQUIRE(ftGroup->getParameter("frames", ftFramesList)); + jointsAndFTs.insert(jointsAndFTs.end(), ftFramesList.begin(), ftFramesList.end()); + } REQUIRE(mdlLdr.loadReducedModelFromFile(modelPath, jointsAndFTs)); } @@ -147,7 +151,7 @@ bool setStaticState(std::shared_ptr kinDyn) iDynTree::make_span(gravity.data(), gravity.size())); } -TEST_CASE("Joint Velocity Dynamics") +TEST_CASE("Joint Velocity Dynamics Without FT") { // Create parameter handler auto parameterHandler = std::make_shared(); @@ -161,6 +165,129 @@ TEST_CASE("Joint Velocity Dynamics") parameterHandler->setParameter("name", name); parameterHandler->setParameter("covariance", covariance); + parameterHandler->setParameter("initial_covariance", covariance); + parameterHandler->setParameter("dynamic_model", model); + parameterHandler->setParameter("elements", elements); + parameterHandler->setParameter("sampling_time", dT); + + // 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("r_leg_ft_acc_bias", 3)); + REQUIRE(variableHandler.addVariable("r_foot_front_ft_acc_bias", 3)); + REQUIRE(variableHandler.addVariable("r_foot_rear_ft_acc_bias", 3)); + REQUIRE(variableHandler.addVariable("r_leg_ft_gyro_bias", 3)); + REQUIRE(variableHandler.addVariable("r_foot_front_ft_gyro_bias", 3)); + REQUIRE(variableHandler.addVariable("r_foot_rear_ft_gyro_bias", 3)); + + auto handlerFromConfig = loadParameterHandler(); + + bool useFT = false; + + iDynTree::ModelLoader modelLoader; + createModelLoader(handlerFromConfig, modelLoader, useFT); + + auto kinDyn = std::make_shared(); + REQUIRE(kinDyn->loadRobotModel(modelLoader.model())); + + REQUIRE(setStaticState(kinDyn)); + + SubModelCreator subModelCreatorWithFT; + createSubModels(modelLoader, kinDyn, handlerFromConfig, useFT, subModelCreatorWithFT); + + std::vector> kinDynWrapperListWithFT; + const auto & subModelListWithFT = subModelCreatorWithFT.getSubModelList(); + + for (int idx = 0; idx < subModelCreatorWithFT.getSubModelList().size(); idx++) + { + kinDynWrapperListWithFT.emplace_back(std::make_shared()); + REQUIRE(kinDynWrapperListWithFT.at(idx)->setKinDyn(kinDyn)); + REQUIRE(kinDynWrapperListWithFT.at(idx)->initialize(subModelListWithFT[idx])); + REQUIRE(kinDynWrapperListWithFT.at(idx)->updateInternalKinDynState(true)); + } + + manif::SE3d::Tangent robotBaseAcceleration; + robotBaseAcceleration.setZero(); + + Eigen::VectorXd robotJointAcceleration(kinDyn->model().getNrOfDOFs()); + robotJointAcceleration.setZero(); + + iDynTree::LinkNetExternalWrenches extWrench(kinDyn->model()); + extWrench.zero(); + + // Compute joint torques in static configuration from inverse dynamics on the full model + iDynTree::FreeFloatingGeneralizedTorques jointTorques(kinDyn->model()); + + kinDyn->inverseDynamics(iDynTree::make_span(robotBaseAcceleration.data(), + manif::SE3d::Tangent::DoF), + robotJointAcceleration, + extWrench, + jointTorques); + + JointVelocityStateDynamics ds; + model = "JointVelocityStateDynamics"; + parameterHandler->setParameter("dynamic_model", model); + REQUIRE(ds.setSubModels(subModelListWithFT, kinDynWrapperListWithFT)); + REQUIRE(ds.initialize(parameterHandler)); + REQUIRE(ds.finalize(variableHandler)); + + Eigen::VectorXd state; + state.resize(variableHandler.getNumberOfVariables()); + state.setZero(); + + int offset = variableHandler.getVariable("tau_m").offset; + int size = variableHandler.getVariable("tau_m").size; + for (int jointIndex = 0; jointIndex < size; jointIndex++) + { + state[offset+jointIndex] = jointTorques.jointTorques()[jointIndex]; + } + + // Create an input for the ukf state + UKFInput input; + + // Define joint positions + Eigen::VectorXd jointPos; + jointPos.resize(kinDyn->model().getNrOfDOFs()); + jointPos.setZero(); + input.robotJointPositions = jointPos; + + // Define base pose + manif::SE3d basePose; + basePose.setIdentity(); + input.robotBasePose = basePose; + + // Define base velocity and acceleration + manif::SE3d::Tangent baseVelocity, baseAcceleration; + baseVelocity.setZero(); + baseAcceleration.setZero(); + input.robotBaseVelocity = baseVelocity; + input.robotBaseAcceleration = baseAcceleration; + + ds.setInput(input); + + ds.setState(state); + + REQUIRE(ds.update()); +} + +TEST_CASE("Joint Velocity Dynamics With FT") +{ + // Create parameter handler + auto parameterHandler = std::make_shared(); + + const std::string name = "ds"; + Eigen::VectorXd covariance(6); + covariance << 1e-3, 1e-3, 5e-3, 5e-3, 5e-3, 5e-3; + std::string model = "JointVelocityStateDynamics"; + const std::vector elements = {"r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll"}; + double dT = 0.01; + + parameterHandler->setParameter("name", name); + parameterHandler->setParameter("covariance", covariance); + parameterHandler->setParameter("initial_covariance", covariance); parameterHandler->setParameter("dynamic_model", model); parameterHandler->setParameter("elements", elements); parameterHandler->setParameter("sampling_time", dT); @@ -187,7 +314,7 @@ TEST_CASE("Joint Velocity Dynamics") auto handlerFromConfig = loadParameterHandler(); iDynTree::ModelLoader modelLoader; - createModelLoader(handlerFromConfig, modelLoader); + createModelLoader(handlerFromConfig, modelLoader, true); auto kinDyn = std::make_shared(); REQUIRE(kinDyn->loadRobotModel(modelLoader.model())); @@ -207,9 +334,27 @@ TEST_CASE("Joint Velocity Dynamics") kinDynWrapperListWithFT.emplace_back(std::make_shared()); REQUIRE(kinDynWrapperListWithFT.at(idx)->setKinDyn(kinDyn)); REQUIRE(kinDynWrapperListWithFT.at(idx)->initialize(subModelListWithFT[idx])); - REQUIRE(kinDynWrapperListWithFT.at(idx)->updateInternalKinDynState()); + REQUIRE(kinDynWrapperListWithFT.at(idx)->updateInternalKinDynState(true)); } + manif::SE3d::Tangent robotBaseAcceleration; + robotBaseAcceleration.setZero(); + + Eigen::VectorXd robotJointAcceleration(kinDyn->model().getNrOfDOFs()); + robotJointAcceleration.setZero(); + + iDynTree::LinkNetExternalWrenches extWrench(kinDyn->model()); + extWrench.zero(); + + // Compute joint torques in static configuration from inverse dynamics on the full model + iDynTree::FreeFloatingGeneralizedTorques jointTorques(kinDyn->model()); + + kinDyn->inverseDynamics(iDynTree::make_span(robotBaseAcceleration.data(), + robotBaseAcceleration.coeffs().size()), + robotJointAcceleration, + extWrench, + jointTorques); + JointVelocityStateDynamics dsSplit; model = "JointVelocityStateDynamics"; parameterHandler->setParameter("dynamic_model", model); @@ -218,46 +363,43 @@ TEST_CASE("Joint Velocity Dynamics") REQUIRE(dsSplit.finalize(variableHandler)); Eigen::VectorXd state; - state.resize(variableHandler.getVariable("r_foot_rear_ft_gyro_bias").offset + variableHandler.getVariable("r_foot_rear_ft_gyro_bias").size + 1); + state.resize(variableHandler.getNumberOfVariables()); state.setZero(); + int offset = variableHandler.getVariable("tau_m").offset; + int size = variableHandler.getVariable("tau_m").size; + for (int jointIndex = 0; jointIndex < size; jointIndex++) + { + state[offset+jointIndex] = jointTorques.jointTorques()[jointIndex]; + } + auto massSecondSubModel = subModelListWithFT.at(1).getModel().getTotalMass(); state(variableHandler.getVariable("r_leg_ft_sensor").offset+2) = massSecondSubModel * BipedalLocomotion::Math::StandardAccelerationOfGravitation; - dsSplit.setState(state); - - REQUIRE(dsSplit.update()); - - std::cout << "Updated state splitting the model" << std::endl; - std::cout << dsSplit.getUpdatedVariable() << std::endl; + // Create an input for the ukf state + UKFInput input; - useFT = false; + // Define joint positions + Eigen::VectorXd jointPos; + jointPos.resize(kinDyn->model().getNrOfDOFs()); + jointPos.setZero(); + input.robotJointPositions = jointPos; - SubModelCreator subModelCreatorWithoutFT; - createSubModels(modelLoader, kinDyn, handlerFromConfig, useFT, subModelCreatorWithoutFT); + // Define base pose + manif::SE3d basePose; + basePose.setIdentity(); + input.robotBasePose = basePose; - std::vector> kinDynWrapperListWithoutFT; - const auto & subModelListWithoutFT = subModelCreatorWithoutFT.getSubModelList(); + // Define base velocity and acceleration + manif::SE3d::Tangent baseVelocity, baseAcceleration; + baseVelocity.setZero(); + baseAcceleration.setZero(); + input.robotBaseVelocity = baseVelocity; + input.robotBaseAcceleration = baseAcceleration; - for (int idx = 0; idx < subModelCreatorWithoutFT.getSubModelList().size(); idx++) - { - kinDynWrapperListWithoutFT.emplace_back(std::make_shared()); - REQUIRE(kinDynWrapperListWithoutFT.at(idx)->setKinDyn(kinDyn)); - REQUIRE(kinDynWrapperListWithoutFT.at(idx)->initialize(subModelListWithoutFT[idx])); - REQUIRE(kinDynWrapperListWithoutFT.at(idx)->updateInternalKinDynState()); - } - - JointVelocityStateDynamics dsNotSplit; - model = "JointVelocityStateDynamics"; - parameterHandler->setParameter("dynamic_model", model); - REQUIRE(dsNotSplit.setSubModels(subModelListWithoutFT, kinDynWrapperListWithoutFT)); - REQUIRE(dsNotSplit.initialize(parameterHandler)); - REQUIRE(dsNotSplit.finalize(variableHandler)); + dsSplit.setInput(input); - dsNotSplit.setState(state); - - REQUIRE(dsNotSplit.update()); + dsSplit.setState(state); - std::cout << "Updated state without splitting the model" << std::endl; - std::cout << dsNotSplit.getUpdatedVariable() << std::endl; + REQUIRE(dsSplit.update()); } diff --git a/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp new file mode 100644 index 0000000000..3798baa365 --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp @@ -0,0 +1,286 @@ +/** + * @file RobotDynamicsEstimatorTest.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include +#include + +//BLF +#include +#include + +#include +#include +#include + +// iDynTree +#include +#include + +//LIBRARYTOTEST +#include +#include +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +IParametersHandler::shared_ptr loadParameterHandler() +{ + std::shared_ptr originalHandler = std::make_shared(); + IParametersHandler::shared_ptr parameterHandler = originalHandler; + + yarp::os::ResourceFinder& rf = yarp::os::ResourceFinder::getResourceFinderSingleton(); + rf.setDefaultConfigFile("/config/config.ini"); + + std::vector arguments = {" ", "--from ", getConfigPath()}; + + std::vector argv; + for (const auto& arg : arguments) + argv.push_back((char*)arg.data()); + argv.push_back(nullptr); + + rf.configure(argv.size() - 1, argv.data()); + + REQUIRE_FALSE(rf.isNull()); + parameterHandler->clear(); + REQUIRE(parameterHandler->isEmpty()); + + originalHandler->set(rf); + + return parameterHandler; +} + +void createModelLoader(IParametersHandler::weak_ptr group, + iDynTree::ModelLoader& mdlLdr) +{ + // List of joints and fts to load the model + std::vector subModelList; + + const std::string modelPath = iCubModels::getModelFile("iCubGenova09"); + + std::vector jointList; + REQUIRE(group.lock()->getParameter("joint_list", jointList)); + + std::vector ftFramesList; + auto ftGroup = group.lock()->getGroup("FT").lock(); + REQUIRE(ftGroup->getParameter("frames", ftFramesList)); + + std::vector jointsAndFTs; + jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); + jointsAndFTs.insert(jointsAndFTs.end(), ftFramesList.begin(), ftFramesList.end()); + + REQUIRE(mdlLdr.loadReducedModelFromFile(modelPath, jointsAndFTs)); +} + +void createInitialState(std::shared_ptr kinDynFullModel, + Eigen::Ref jointTorques, + IParametersHandler::weak_ptr modelHandler, + const std::vector& subModelList, + Output& initialState) +{ + // All the values set here come from an experiment on the real robot + + // Resize variables + initialState.ds.resize(kinDynFullModel->model().getNrOfDOFs()); // ds + initialState.tau_m.resize(kinDynFullModel->model().getNrOfDOFs()); // tau_m + initialState.tau_F.resize(kinDynFullModel->model().getNrOfDOFs()); // tau_F + std::vector ftList; + auto ftGroup = modelHandler.lock()->getGroup("FT").lock(); + REQUIRE(ftGroup->getParameter("names", ftList)); + for (auto ft : ftList) + { + initialState.ftWrenches[ft] = Eigen::VectorXd(6).setZero(); // FT + + std::string ftBias = ft + "_bias"; + initialState.ftWrenchesBiases[ftBias] = Eigen::VectorXd(6).setZero(); // FT bias + } + std::vector accList; + auto accGroup = modelHandler.lock()->getGroup("ACCELEROMETER").lock(); + REQUIRE(accGroup->getParameter("names", accList)); + for (auto acc : accList) + { + std::string accBias = acc + "_bias"; + initialState.accelerometerBiases[accBias] = Eigen::VectorXd(3).setZero(); // ACC BIAS + } + std::vector gyroList; + auto gyroGroup = modelHandler.lock()->getGroup("GYROSCOPE").lock(); + REQUIRE(gyroGroup->getParameter("names", gyroList)); + for (auto gyro : gyroList) + { + std::string gyroBias = gyro + "_bias"; + initialState.gyroscopeBiases[gyroBias] = Eigen::VectorXd(3).setZero(); // GYRO BIAS + } + + + // Set values + initialState.ds.setZero(); + initialState.tau_m << -1.24764e+01, 1.03400e-01, -4.70000e-03, -3.16350e+00, 4.21800e-01, 4.85000e-01; + initialState.tau_F.setZero(); + initialState.ftWrenches["r_leg_ft_sensor"] << -3.83709731e+01, -8.45653659e+00, 9.25208925e+01, 3.12676978e+00, -9.79714657e+00, 4.01285264e-01; + initialState.ftWrenches["r_foot_front_ft_sensor"] << 5.22305136e-01, -4.88067107e-01, 1.61011089e+00, -9.13330381e-03, -8.39265034e-03, 4.18725767e-04; + initialState.ftWrenches["r_foot_rear_ft_sensor"] << 5.19688377e-01, -4.85621881e-01, 1.60204420e+00, -8.32204636e-03, -9.16952530e-03, -7.99299795e-05; + initialState.ftWrenchesBiases["r_leg_ft_sensor_bias"] << -1.89410386e+01, 7.33674253e+01, -6.04774355e+01, 1.34890092e-02, -3.02023625e+00, 7.01925185e-01; + initialState.ftWrenchesBiases["r_foot_front_ft_sensor_bias"] << -4.85874907e+01, -3.90627141e+01, -3.89636265e+01, -1.83127438e-01, 9.51385814e-01, 2.97127661e-01; + initialState.ftWrenchesBiases["r_foot_rear_ft_sensor_bias"] << -3.75985458e+01, -6.87282453e+01, -1.41893873e+00, -2.24845286e+00, 1.18104453e+00, 3.75446141e-01; +} + +void createInput(std::shared_ptr kinDynFullModel, + Eigen::Ref jointTorques, + IParametersHandler::weak_ptr modelHandler, + const std::vector& subModelList, + Input& input) +{ + // All the values set here come from an experiment on the real robot + + manif::SE3d basePose; + basePose.setIdentity(); + input.basePose = basePose; + + manif::SE3d::Tangent baseVel; + baseVel.setZero(); + input.baseVelocity = baseVel; + + manif::SE3d::Tangent baseAcc; + baseAcc.setZero(); + input.baseAcceleration = baseAcc; + + input.jointPositions.resize(kinDynFullModel->model().getNrOfDOFs()); + input.jointPositions << -3.81866275e-01, 1.27512464e-01, 3.83496133e-04, -2.67488553e-02, -9.77915140e-03, 9.58740333e-05; + + input.jointVelocities.resize(kinDynFullModel->model().getNrOfDOFs()); + input.jointVelocities.setZero(); + + // Use gearbox ratio and torque constants to convert joint torques into motor currents + Eigen::VectorXd gearRatio(kinDynFullModel->model().getNrOfDOFs()); + gearRatio << 100.0, -100.0, 100.0, 100.0, 100.0, 100.0; + + Eigen::VectorXd torqueConstant(kinDynFullModel->model().getNrOfDOFs()); + torqueConstant << 0.111, 0.047, 0.047, 0.111, 0.111, 0.025; + +// input.motorCurrents = jointTorques.array() / (gearRatio.array() * torqueConstant.array()); + input.motorCurrents.resize(kinDynFullModel->model().getNrOfDOFs()); + input.motorCurrents << -1.124e+00, -2.200e-02, -1.000e-03, -2.850e-01, 3.800e-02, 1.940e-01; + + input.ftWrenches["r_leg_ft_sensor"] = Eigen::VectorXd(6).setZero(); +// input.ftWrenches["r_leg_ft_sensor"](2) = subModelList.at(1).getModel().getTotalMass() * BipedalLocomotion::Math::StandardAccelerationOfGravitation; + input.ftWrenches["r_leg_ft_sensor"] << -57.31201172, 64.91088867, 32.04345703, 3.14025879, -12.81738281, 1.10321045; + + input.ftWrenches["r_foot_front_ft_sensor"] = Eigen::VectorXd(6).setZero(); +// input.ftWrenches["r_foot_front_ft_sensor"](2) = subModelList.at(2).getModel().getTotalMass() * BipedalLocomotion::Math::StandardAccelerationOfGravitation; + input.ftWrenches["r_foot_front_ft_sensor"] << -48.06518555, -39.55078125, -37.35351562, -0.19226074, 0.94299316, 0.29754639; + + input.ftWrenches["r_foot_rear_ft_sensor"] = Eigen::VectorXd(6).setZero(); +// input.ftWrenches["r_foot_rear_ft_sensor"](2) = subModelList.at(3).getModel().getTotalMass() * BipedalLocomotion::Math::StandardAccelerationOfGravitation; + input.ftWrenches["r_foot_rear_ft_sensor"] << -37.07885742, -69.21386719, 0.18310547, -2.2567749, 1.171875, 0.37536621; + + std::vector accList; + auto accGroup = modelHandler.lock()->getGroup("ACCELEROMETER").lock(); + REQUIRE(accGroup->getParameter("names", accList)); + for (auto acc : accList) + { + input.linearAccelerations[acc] = Eigen::VectorXd(3).setZero(); + input.linearAccelerations[acc](2) = - BipedalLocomotion::Math::StandardAccelerationOfGravitation; + } + input.linearAccelerations["r_leg_ft_acc"] << 3.72, 0.8, -9.33; + input.linearAccelerations["r_foot_front_ft_acc"] << 4.01, -2.38, 8.74; + input.linearAccelerations["r_foot_rear_ft_acc"] << 3.77, -2.39, 8.94; + + std::vector gyroList; + auto gyroGroup = modelHandler.lock()->getGroup("GYROSCOPE").lock(); + REQUIRE(gyroGroup->getParameter("names", gyroList)); + for (auto gyro : gyroList) + { + input.angularVelocities[gyro] = Eigen::VectorXd(3).setZero(); + } + input.angularVelocities["r_leg_ft_gyro"] << 0.03708825, -0.06654068, -0.00109083; + input.angularVelocities["r_foot_front_ft_gyro"] << 0.0567232 , 0.04799655, -0.01308997; + input.angularVelocities["r_foot_rear_ft_gyro"] << 0.06108652, 0.04690572, -0.01090831; +} + +TEST_CASE("RobotDynamicsEstimator") +{ + auto parameterHandler = loadParameterHandler(); + + iDynTree::ModelLoader modelLoader; + auto modelHandler = parameterHandler->getGroup("MODEL").lock(); + + REQUIRE_FALSE(modelHandler == nullptr); + + createModelLoader(modelHandler, modelLoader); + + auto kinDyn = std::make_shared(); + REQUIRE(kinDyn->loadRobotModel(modelLoader.model())); + REQUIRE(kinDyn->setFrameVelocityRepresentation(iDynTree::BODY_FIXED_REPRESENTATION)); + + SubModelCreator subModelCreator; + subModelCreator.setModelAndSensors(kinDyn->model(), modelLoader.sensors()); + REQUIRE(subModelCreator.setKinDyn(kinDyn)); + + REQUIRE(subModelCreator.createSubModels(modelHandler)); + + std::vector> kinDynWrapperList; + + const auto & subModelList = subModelCreator.getSubModelList(); + + for (int idx = 0; idx < subModelList.size(); idx++) + { + kinDynWrapperList.emplace_back(std::make_shared()); + REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); + REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); + REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState(true)); + } + + // automatic build the Estimator from parameter handler + auto estimator = RobotDynamicsEstimator::build(parameterHandler, kinDyn, subModelList, kinDynWrapperList); + REQUIRE_FALSE(estimator == nullptr); + + manif::SE3d::Tangent robotBaseAcceleration; + robotBaseAcceleration.setZero(); + + Eigen::VectorXd robotJointAcceleration(kinDyn->model().getNrOfDOFs()); + robotJointAcceleration.setZero(); + + iDynTree::LinkNetExternalWrenches extWrench(kinDyn->model()); + extWrench.zero(); + + // Compute joint torques in static configuration from inverse dynamics on the full model + iDynTree::FreeFloatingGeneralizedTorques jointTorques(kinDyn->model()); + + kinDyn->inverseDynamics(iDynTree::make_span(robotBaseAcceleration.data(), + manif::SE3d::Tangent::DoF), + robotJointAcceleration, + extWrench, + jointTorques); + + Output initialState; + createInitialState(kinDyn, + iDynTree::toEigen(jointTorques.jointTorques()), + modelHandler, + subModelList, + initialState); + + REQUIRE(estimator->setInitialState(initialState)); + + Input measurement; + createInput(kinDyn, + iDynTree::toEigen(jointTorques.jointTorques()), + modelHandler, + subModelList, + measurement); + + auto tic = BipedalLocomotion::clock().now(); + REQUIRE(estimator->setInput(measurement)); + REQUIRE(estimator->advance()); + auto toc = BipedalLocomotion::clock().now(); + + BipedalLocomotion::log()->error("{}", toc - tic); + + Output result = estimator->getOutput(); +} diff --git a/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp index 0c5ccc7561..1d865c515a 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp @@ -219,7 +219,7 @@ TEST_CASE("SubModelKinDynWrapper") REQUIRE(kinDynSubModel.initialize(subModelList[idx])); - REQUIRE(kinDynSubModel.updateInternalKinDynState()); + REQUIRE(kinDynSubModel.updateInternalKinDynState(true)); int numberOfJoints = subModelList[idx].getModel().getNrOfDOFs(); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/UkfMeasurementTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/UkfMeasurementTest.cpp index a757049f89..acbec5c379 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/UkfMeasurementTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/UkfMeasurementTest.cpp @@ -108,7 +108,7 @@ TEST_CASE("UkfMeasurement") kinDynWrapperList.emplace_back(std::make_shared()); REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); - REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState()); + REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState(true)); } // Build the UkfState @@ -243,7 +243,7 @@ TEST_CASE("UkfMeasurement") REQUIRE(resultOk); - Eigen::VectorXd updatedMeasurement = bfl::any::any_cast(std::move(updatedMeasurementTmp)); + Eigen::VectorXd updatedMeasurement = bfl::any::any_cast(std::move(updatedMeasurementTmp)); for (int idx = 0; idx < updatedMeasurement.size(); idx++) { diff --git a/src/Estimators/tests/RobotDynamicsEstimator/UkfStateTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/UkfStateTest.cpp index 0597c83dbd..d12b8e703a 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/UkfStateTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/UkfStateTest.cpp @@ -108,7 +108,7 @@ TEST_CASE("UkfState") kinDynWrapperList.emplace_back(std::make_shared()); REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); - REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState()); + REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState(true)); } // Build the UkfState diff --git a/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityDynamicsTest.cpp index 667d39f73b..b370ed9a57 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityDynamicsTest.cpp @@ -33,6 +33,7 @@ TEST_CASE("Zero Velocity Dynamics") parameterHandler->setParameter("name", name0); parameterHandler->setParameter("covariance", covariance0); + parameterHandler->setParameter("initial_covariance", covariance0); parameterHandler->setParameter("dynamic_model", model0); parameterHandler->setParameter("sampling_time", 0.01); @@ -81,6 +82,7 @@ TEST_CASE("Zero Velocity Dynamics") parameterHandler->clear(); parameterHandler->setParameter("name", name1); parameterHandler->setParameter("covariance", covariance1); + parameterHandler->setParameter("initial_covariance", covariance1); parameterHandler->setParameter("dynamic_model", model1); parameterHandler->setParameter("sampling_time", 0.01); parameterHandler->setParameter("use_bias", useBias); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf.ini index a9b0ac35b5..6805ff7bd6 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf.ini +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf.ini @@ -1,3 +1,7 @@ +alpha 1.0 +beta 2.0 +kappa 0.0 + [include UKF_STATE "ukf/ukf_state.ini"] [include UKF_MEASUREMENT "ukf/ukf_measurement.ini"] diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini index db42018a42..10ac7c8ae5 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini @@ -1,4 +1,7 @@ -dynamics_list ("JOINT_VELOCITY", "MOTOR_CURRENT", "RIGHT_LEG_FT", "RIGHT_FOOT_FRONT_FT", "RIGHT_FOOT_REAR_FT", "RIGHT_LEG_ACC", "RIGHT_LEG_GYRO") +dynamics_list ("JOINT_VELOCITY", "MOTOR_CURRENT", + "RIGHT_LEG_FT", "RIGHT_FOOT_FRONT_FT", "RIGHT_FOOT_REAR_FT", + "RIGHT_LEG_ACC", "RIGHT_FOOT_FRONT_ACC", "RIGHT_FOOT_REAR_ACC", + "RIGHT_LEG_GYRO", "RIGHT_FOOT_FRONT_GYRO", "RIGHT_FOOT_REAR_GYRO") [JOINT_VELOCITY] name "ds" @@ -42,6 +45,20 @@ covariance (2.3e-3, 1.9e-3, 3.1e-3) use_bias 1 dynamic_model "AccelerometerMeasurementDynamics" +[RIGHT_FOOT_FRONT_ACC] +name "r_leg_ft_acc" +elements ("x", "y", "z") +covariance (2.3e-3, 1.9e-3, 3.1e-3) +use_bias 1 +dynamic_model "AccelerometerMeasurementDynamics" + +[RIGHT_FOOT_REAR_ACC] +name "r_leg_ft_acc" +elements ("x", "y", "z") +covariance (2.5e-3, 2.3e-3, 3e-3) +use_bias 1 +dynamic_model "AccelerometerMeasurementDynamics" + [RIGHT_LEG_GYRO] name "r_leg_ft_gyro" elements ("x", "y", "z") @@ -49,23 +66,16 @@ covariance (0.000000283, 0.0127, 0.0000001) use_bias 1 dynamic_model "GyroscopeMeasurementDynamics" -#[ACCELEROMETER] -#name ("r_leg_ft_acc", "r_foot_front_ft_acc", "r_foot_rear_ft_acc") -#elements ("x", "y", "z") -#covariance (2.3e-3, 1.9e-3, 3.1e-3, -# 2.7e-3, 1.99e-3, 2.78e-3, -# 2.5e-3, 2.3e-3, 3e-3) -#use_bias 1 -#dynamic_model "AccelerometerMeasurementDynamics" - -#[GYROSCOPE] -#name ("r_leg_ft_gyro", "r_foot_front_ft_gyro", "r_foot_rear_ft_gyro") -#elements ("x", "y", "z") -#covariance (0.000000283, 0.0127, 0.0000001, -# 4.1e-4, 3.3e-4, 3.2e-4, -# 4.9e-4, 4.2e-4, 3.3e-4) -#use_bias 1 -#dynamic_model "GyroscopeMeasurementDynamics" +[RIGHT_FOOT_FRONT_GYRO] +name "r_leg_ft_gyro" +elements ("x", "y", "z") +covariance (4.1e-4, 3.3e-4, 3.2e-4) +use_bias 1 +dynamic_model "GyroscopeMeasurementDynamics" -# "RIGHT_LEG_ACC", "RIGHT_FOOT_FRONT_ACC", "RIGHT_FOOT_REAR_ACC", "RIGHT_LEG_GYRO", -# "RIGHT_FOOT_FRONT_GYRO", "RIGHT_FOOT_REAR_GYRO") +[RIGHT_FOOT_REAR_GYRO] +name "r_leg_ft_gyro" +elements ("x", "y", "z") +covariance (4.9e-4, 4.2e-4, 3.3e-4) +use_bias 1 +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 3d94cb90e9..a39ef7c455 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini @@ -7,92 +7,107 @@ dynamics_list ("JOINT_VELOCITY", "MOTOR_TORQUE", "FRICTION_TORQUE", "RIGHT_LEG_F name "ds" elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") covariance (1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3) +initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) dynamic_model "JointVelocityStateDynamics" [MOTOR_TORQUE] name "tau_m" elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") covariance (1e-3, 1e-3, 5e-3, 5e-3, 5e-3, 5e-3) +initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) dynamic_model "ZeroVelocityDynamics" [FRICTION_TORQUE] name "tau_F" elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") covariance (1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-1) -dynamic_model "FrictionTorqueStateDynamics" +dynamic_model "ZeroVelocityDynamics" friction_k0 (9.106, 5.03, 4.93, 12.88, 14.34, 1.12) friction_k1 (200.0, 6.9, 200.0, 59.87, 200.0, 200.0) friction_k2 (1.767, 5.64, 0.27, 2.0, 3.0, 0.0) +initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) [RIGHT_LEG_FT] name "r_leg_ft_sensor" elements ("fx", "fy", "fz", "mx", "my", "mz") covariance (1e-3, 1e-3, 1e-3, 1e-4, 1e-4, 1e-4) +initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) dynamic_model "ZeroVelocityDynamics" [RIGHT_FOOT_FRONT_FT] name "r_foot_front_ft_sensor" elements ("fx", "fy", "fz", "mx", "my", "mz") covariance (1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6) +initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) dynamic_model "ZeroVelocityDynamics" [RIGHT_FOOT_REAR_FT] name "r_foot_rear_ft_sensor" elements ("fx", "fy", "fz", "mx", "my", "mz") covariance (1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6) +initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) dynamic_model "ZeroVelocityDynamics" [RIGHT_LEG_FT_BIAS] name "r_leg_ft_sensor_bias" elements ("fx", "fy", "fz", "mx", "my", "mz") covariance (1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6) +initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) dynamic_model "ZeroVelocityDynamics" [RIGHT_FOOT_FRONT_FT_BIAS] name "r_foot_front_ft_sensor_bias" elements ("fx", "fy", "fz", "mx", "my", "mz") covariance (1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6) +initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) dynamic_model "ZeroVelocityDynamics" [RIGHT_FOOT_REAR_FT_BIAS] name "r_foot_rear_ft_sensor_bias" elements ("fx", "fy", "fz", "mx", "my", "mz") covariance (1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6) +initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) dynamic_model "ZeroVelocityDynamics" [RIGHT_LEG_ACC_BIAS] name "r_leg_ft_acc_bias" elements ("x", "y", "z") covariance (7.9e-5, 1.9e-5, 4.4e-5) +initial_covariance (0.01, 0.01, 0.01) dynamic_model "ZeroVelocityDynamics" [RIGHT_FOOT_FRONT_ACC_BIAS] name "r_foot_front_ft_acc_bias" elements ("x", "y", "z") covariance (7.4e-5, 1.2e-5, 4.4e-5) +initial_covariance (0.01, 0.01, 0.01) dynamic_model "ZeroVelocityDynamics" [RIGHT_FOOT_REAR_ACC_BIAS] name "r_foot_rear_ft_acc_bias" elements ("x", "y", "z") covariance (5.5e-5, 1.1e-5, 1.7e-5) +initial_covariance (0.01, 0.01, 0.01) dynamic_model "ZeroVelocityDynamics" [RIGHT_LEG_GYRO_BIAS] name "r_leg_ft_gyro_bias" elements ("x", "y", "z") covariance (4e-4, 4.7e-4, 4.7e-4) +initial_covariance (0.01, 0.01, 0.01) dynamic_model "ZeroVelocityDynamics" [RIGHT_FOOT_FRONT_GYRO_BIAS] name "r_foot_front_ft_gyro_bias" elements ("x", "y", "z") covariance (1.3e-2, 9.9e-3, 9e-3) +initial_covariance (0.01, 0.01, 0.01) dynamic_model "ZeroVelocityDynamics" [RIGHT_FOOT_REAR_GYRO_BIAS] name "r_foot_rear_ft_gyro_bias" elements ("x", "y", "z") covariance (8.2e-8, 1e-2, 9.3e-3) +initial_covariance (0.01, 0.01, 0.01) dynamic_model "ZeroVelocityDynamics" From 7a094f78804622891db7765489302484b510128c Mon Sep 17 00:00:00 2001 From: Ines Date: Mon, 17 Apr 2023 14:59:26 +0200 Subject: [PATCH 06/13] Fix use of the joints to split the model into sub-models --- .../RobotDynamicsEstimator/SubModel.h | 5 +-- src/Estimators/src/SubModel.cpp | 35 +++++++++++++------ 2 files changed, 27 insertions(+), 13 deletions(-) diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h index 0c4bb0c5e2..97893b9ceb 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h @@ -56,6 +56,7 @@ struct FT : Sensor Direction forceDirection = Direction::NotSpecified; /**< Force direction depending on which side of the sensor is considered (+1 or -1)*/ + std::string associatedJoint; /**< Name of the fixd joint used to represent the ft sensor in the model. */ }; /** @@ -311,13 +312,13 @@ class SubModelCreator /** * @brief createSubModels splits the model in SubModel objects cutting the model at the * force/torque sensors specified by the parameterHandler. - * @param ftSensorList list of Sensor structs. + * @param ftSensorList list of FT structs. * @param accList list of Sensor structs. * @param gyroList list of Sensor structs. * @param externalContacts list of strings. * @return a boolean value saying if the subModelList has been created correctly. */ - bool createSubModels(const std::vector& ftSensorList, + bool createSubModels(const std::vector& ftSensorList, const std::vector& accList, const std::vector& gyroList, const std::vector& externalContacts); diff --git a/src/Estimators/src/SubModel.cpp b/src/Estimators/src/SubModel.cpp index 18f3c37c53..fd7aa6e338 100644 --- a/src/Estimators/src/SubModel.cpp +++ b/src/Estimators/src/SubModel.cpp @@ -107,6 +107,8 @@ RDE::SubModelCreator::attachFTsToSubModel(iDynTree::Model& idynSubModel) ft.forceDirection = RDE::FT::Direction::Negative; } + log()->error("Creating Ft --> {}", ft.name); + ftList.push_back(ft); } else { @@ -154,6 +156,8 @@ RDE::SubModelCreator::attachFTsToSubModel(iDynTree::Model& idynSubModel) ft.forceDirection = RDE::FT::Direction::Negative; } + log()->error("Creating Ft --> {}", ft.name); + ftList.push_back(std::move(ft)); } } @@ -257,23 +261,23 @@ RDE::SubModelCreator::populateSubModel(iDynTree::Model& idynSubModel, return subModel; } -bool RDE::SubModelCreator::createSubModels(const std::vector& ftSensorList, +bool RDE::SubModelCreator::createSubModels(const std::vector& ftSensorList, const std::vector& accList, const std::vector& gyroList, const std::vector& externalContacts) { constexpr auto logPrefix = "[BipedalLocomotion::RobotDynamicsEstimator::SubModelCreator::" - "getSubModels]"; + "createSubModels]"; // Split model in submodels - std::vector ftNameList; + std::vector ftList; for (auto idx = 0; idx < ftSensorList.size(); idx++) { - ftNameList.push_back(ftSensorList[idx].frame); + ftList.push_back(ftSensorList[idx].associatedJoint); } std::vector idynSubModels; - if (!this->splitModel(ftNameList, idynSubModels)) + if (!this->splitModel(ftList, idynSubModels)) { blf::log()->error("{} Unable to split the model in submodels.", logPrefix); return false; @@ -318,8 +322,8 @@ bool RDE::SubModelCreator::createSubModels( } auto populateSensorParameters = [&ptr, logPrefix](const std::string& groupName, - std::vector& names, - std::vector& frames) -> bool { + std::vector& names, + std::vector& frames) -> bool { auto group = ptr->getGroup(groupName).lock(); if (group == nullptr) { @@ -362,15 +366,24 @@ bool RDE::SubModelCreator::createSubModels( logPrefix); } - std::vector ftNames, ftFrames; + std::vector ftNames, ftFrames, ftAssociatedJoints; bool ok = populateSensorParameters("FT", ftNames, ftFrames); - std::vector ftList; + auto ftGroup = ptr->getGroup("FT").lock(); + if (ftGroup == nullptr) + { + blf::log()->error("{} Unable to get the group names 'FT'.", logPrefix); + return false; + } + ok = ok && ftGroup->getParameter("associated_joints", ftAssociatedJoints); + + std::vector ftList; for (auto idx = 0; idx < ftNames.size(); idx++) { - RDE::Sensor ft; + RDE::FT ft; ft.name = ftNames[idx]; ft.frame = ftFrames[idx]; + ft.associatedJoint = ftAssociatedJoints[idx]; ftList.push_back(std::move(ft)); } @@ -399,7 +412,7 @@ bool RDE::SubModelCreator::createSubModels( } ok = ok - && RDE::SubModelCreator::createSubModels(ftList, accList, gyroList, extContactFrames); + && RDE::SubModelCreator::createSubModels(ftList, accList, gyroList, extContactFrames); return ok; } From d8d692f4c0c8954d7ecaec1d5eb2a1742642c18b Mon Sep 17 00:00:00 2001 From: Ines Date: Thu, 13 Apr 2023 19:35:31 +0200 Subject: [PATCH 07/13] Move forward dynamics from dynamics classes to ukfstate and ukfmeasurement classes. Fix accelerometer dynamic model formula. --- src/Estimators/CMakeLists.txt | 4 +- .../AccelerometerMeasurementDynamics.h | 15 +- .../RobotDynamicsEstimator/Dynamics.h | 1 + .../FrictionTorqueStateDynamics.h | 14 +- .../JointVelocityStateDynamics.h | 8 - .../SubModelKinDynWrapper.h | 36 +++-- .../RobotDynamicsEstimator/UkfState.h | 3 + .../src/AccelerometerMeasurementDynamics.cpp | 141 ++++++----------- .../src/FrictionTorqueStateDynamics.cpp | 131 +-------------- .../src/JointVelocityStateDynamics.cpp | 121 +------------- src/Estimators/src/SubModelDynamics.cpp | 7 +- src/Estimators/src/SubModelKinDynWrapper.cpp | 56 ++++--- src/Estimators/src/UkfMeasurement.cpp | 146 ++++++++++++++++- src/Estimators/src/UkfState.cpp | 149 ++++++++++++++++-- .../AccelerometerMeasurementDynamicsTest.cpp | 10 +- .../FrictionTorqueStateDynamicsTest.cpp | 57 ++++--- .../GyroscopeMeasurementDynamicsTest.cpp | 8 +- .../JointVelocityStateDynamicsTest.cpp | 29 +++- .../RobotDynamicsEstimatorTest.cpp | 8 +- .../SubModelKinDynWrapperTest.cpp | 8 +- .../UkfMeasurementTest.cpp | 8 +- .../RobotDynamicsEstimator/UkfStateTest.cpp | 8 +- 22 files changed, 509 insertions(+), 459 deletions(-) diff --git a/src/Estimators/CMakeLists.txt b/src/Estimators/CMakeLists.txt index 995269e7b3..8640170bc6 100644 --- a/src/Estimators/CMakeLists.txt +++ b/src/Estimators/CMakeLists.txt @@ -28,13 +28,13 @@ if(FRAMEWORK_COMPILE_RobotDynamicsEstimator) add_bipedal_locomotion_library( NAME RobotDynamicsEstimator SOURCES src/SubModel.cpp src/SubModelKinDynWrapper.cpp src/Dynamics.cpp src/ZeroVelocityDynamics.cpp src/FrictionTorqueStateDynamics.cpp - src/JointVelocityStateDynamics.cpp src/UkfState.cpp src/SubModelDynamics.cpp src/MotorCurrentMeasurementDynamics.cpp + src/JointVelocityStateDynamics.cpp src/UkfState.cpp src/MotorCurrentMeasurementDynamics.cpp src/UkfMeasurement.cpp src/AccelerometerMeasurementDynamics.cpp src/GyroscopeMeasurementDynamics.cpp src/RobotDynamicsEstimator.cpp SUBDIRECTORIES tests/RobotDynamicsEstimator PUBLIC_HEADERS ${H_PREFIX}/SubModel.h ${H_PREFIX}/SubModelKinDynWrapper.h ${H_PREFIX}/Dynamics.h ${H_PREFIX}/ZeroVelocityDynamics.h ${H_PREFIX}/FrictionTorqueStateDynamics.h ${H_PREFIX}/JointVelocityStateDynamics.h ${H_PREFIX}/AccelerometerMeasurementDynamics.h - ${H_PREFIX}/UkfState.h ${H_PREFIX}/SubModelDynamics.h ${H_PREFIX}/MotorCurrentMeasurementDynamics.h ${H_PREFIX}/UkfMeasurement.h + ${H_PREFIX}/UkfState.h ${H_PREFIX}/MotorCurrentMeasurementDynamics.h ${H_PREFIX}/UkfMeasurement.h ${H_PREFIX}/GyroscopeMeasurementDynamics.h ${H_PREFIX}/RobotDynamicsEstimator.h PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler MANIF::manif BipedalLocomotion::System BipedalLocomotion::Math iDynTree::idyntree-high-level iDynTree::idyntree-core iDynTree::idyntree-model iDynTree::idyntree-modelio Eigen3::Eigen MANIF::manif BayesFilters::BayesFilters diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h index 9652528163..843a8e2358 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h @@ -38,25 +38,24 @@ 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 m_subModelList; /** List of SubModel objects. */ + std::vector> m_kinDynWrapperList; /**< List of pointers to SubModelKinDynWrapper objects. */ bool m_isSubModelListSet{false}; /**< Boolean flag saying if the sub-model list has been set. */ double m_dT; /**< Sampling time. */ - int m_nrOfSubDynamics; /**< Number of sub-dynamics which corresponds to the number of sub-models. */ - std::vector> m_subDynamics; /**< Vector of SubModelInversDynamics objects. */ - Eigen::VectorXd m_jointVelocityFullModel; /**< Vector of joint velocities. */ - Eigen::VectorXd m_motorTorqueFullModel; /**< Motor torque vector of full-model. */ - Eigen::VectorXd m_frictionTorqueFullModel; /**< Friction torque vector of full-model. */ std::vector m_subModelJointAcc; /**< Updated joint acceleration of each sub-model. */ - Eigen::VectorXd m_jointAccelerationFullModel; /**< Vector of joint accelerations. */ Eigen::Vector3d m_gravity; /**< Gravitational acceleration. */ - std::vector m_subModelsWithAcc; /**< List of indeces saying which sub-model in the m_subDynamics list containa the accelerometer. */ + std::vector m_subModelsWithAccelerometer; /**< List of indeces saying which sub-model in the m_subDynamics list containa the accelerometer. */ + manif::SE3d::Tangent m_accFrameVel; /**< Velocity at the accelerometer frame. */ protected: Eigen::VectorXd m_covSingleVar; - manif::SE3d::Tangent m_subModelBaseAcc; Eigen::VectorXd m_JdotNu; Eigen::VectorXd m_JvdotBase; Eigen::VectorXd m_Jsdotdot; Eigen::Vector3d m_accRg; + Eigen::Vector3d m_vCrossW; + Eigen::Vector3d linVel; + Eigen::Vector3d angVel; public: /* diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h index 4f7beb2597..1b9d220d02 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h @@ -49,6 +49,7 @@ namespace RobotDynamicsEstimator struct UKFInput { Eigen::VectorXd robotJointPositions; /**< Vector of joint positions. */ + Eigen::VectorXd robotJointAccelerations; /**< Vector of joint accelerations. */ manif::SE3d robotBasePose; /**< Robot base position and orientation. */ manif::SE3d::Tangent robotBaseVelocity; /**< Robot base velocity. */ manif::SE3d::Tangent robotBaseAcceleration; /**< Robot base acceleration. */ diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h index 102e48139f..efa966e0b8 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h @@ -32,29 +32,21 @@ namespace RobotDynamicsEstimator * \f] * In the discrete time the dynamics is defined as: * \f[ - * \tau_{F,k+1} = \tau_{F,k} + \Delta T ( k_{2} + k_{0} k_{1} (1 - tanh^{2} (k_{1} \dot{s,k})) ) \ddot{s} + * \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_currentFrictionTorque; /**< Vector of friction torques. */ Eigen::VectorXd m_k0, m_k1, m_k2; /**< Friction parameters (see class description). */ double m_dT; /**< Sampling time. */ - int m_nrOfSubDynamics; /**< Number of sub-dynamics which corresponds to the number of sub-models. */ - std::vector> m_subDynamics; /**< Vector of SubModelInversDynamics objects. */ - Eigen::VectorXd m_motorTorqueFullModel; /**< Motor torque vector of full-model. */ Eigen::VectorXd m_frictionTorqueFullModel; /**< Friction torque vector of full-model. */ - std::vector m_subModelUpdatedJointAcceleration; /**< Updated joint acceleration of each sub-model. */ - Eigen::VectorXd m_jointAccelerationFullModel; /**< Vector of joint accelerations. */ - bool m_isSubModelListSet{false}; /**< Boolean flag saying if the sub-model list has been set. */ protected: - Eigen::VectorXd m_tanhArgument; - Eigen::VectorXd m_tanh; + Eigen::VectorXd m_coshArgument; + Eigen::VectorXd m_coshsquared; Eigen::VectorXd m_k0k1; - Eigen::VectorXd m_argParenthesis; Eigen::VectorXd m_dotTauF; public: diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h index 40a295ba48..6a459671fa 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h @@ -36,16 +36,8 @@ namespace RobotDynamicsEstimator class JointVelocityStateDynamics : public Dynamics { - int m_nrOfSubDynamics; /**< Number of sub-dynamics which corresponds to the number of sub-models. */ - std::vector> m_subDynamics; /**< Vector of SubModelInversDynamics objects. */ double m_dT; /**< Sampling time. */ - bool m_isSubModelListSet{false}; /**< Boolean flag saying if the sub-model list has been set. */ Eigen::VectorXd m_jointVelocityFullModel; /**< Joint velocities of full-model. */ - Eigen::VectorXd m_motorTorqueFullModel; /**< Motor torque vector of full-model. */ - Eigen::VectorXd m_frictionTorqueFullModel; /**< Friction torque vector of full-model. */ - std::vector m_subModelUpdatedJointVelocity; /**< Updated joint velocity of each sub-model. */ - Eigen::VectorXd m_jointAccelerationFullModel; /**< Vector of joint accelerations. */ - std::vector m_subModelUpdatedJointAcceleration; /**< Updated joint acceleration of each sub-model. */ public: /* diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h index a50cfabc5e..3d3f734165 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h @@ -53,11 +53,15 @@ class SubModelKinDynWrapper */ std::map m_accRworldList; /**< Rotation matrix of the accelerometer frame wrt world */ + std::map m_accVelList; /**< Acceleration of the + accelerometers */ manif::SE3d::Tangent m_baseVelocity; /**< Velocity of the base of the sub-model */ std::string m_baseFrame; /**< Name of the base frame of the sub-model */ std::shared_ptr m_kinDynFullModel; + manif::SE3d::Tangent m_subModelBaseAcceleration; /** Acceleration of the sub-model base. */ + protected: int m_numOfJoints; /**< Number of joints in the sub-model */ Eigen::MatrixXd m_FTranspose; /**< It is the bottom-left block of the mass matrix, that is, @@ -83,6 +87,11 @@ class SubModelKinDynWrapper */ bool updateDynamicsVariableState(bool isCorrectStep); + /** + * @brief Compute the contribution of external contacts on the joint torques. + */ + void computeTotalTorqueFromContacts(); + public: /** * @brief set kinDyn @@ -102,10 +111,15 @@ class SubModelKinDynWrapper initialize(const SubModel& subModel); /** - * @brief updateInternalKinDynState updates the state of the KinDynWrapper object. + * @brief updateState updates the state of the KinDynWrapper object. + * @param robotBaseAcceleration is a manif::SE3d::Tangent representing the robot base acceleration. + * @param robotJointAcceleration is a Eigen reference to a Eigen::VectorXd containing the joint accelerations. + * @param isCorrectStep is a boolean saying if the method is called during the predict step or the correct step. * @return a boolean value saying if the subModelList has been created correctly. */ - bool updateInternalKinDynState(bool isCorrectStep); + bool updateState(manif::SE3d::Tangent& robotBaseAcceleration, + Eigen::Ref robotJointAcceleration, + bool isCorrectStep); /** * @brief forwardDynamics computes the free floaing forward dynamics @@ -125,18 +139,13 @@ class SubModelKinDynWrapper /** * @brief getBaseAcceleration gets the acceleration of the sub-model base. - * @param robotBaseAcceleration the acceleration of the robot base. - * @param robotJointAcceleration the acceleration of the robot joints. - * @param subModelBaseAcceleration the base acceleration of the sub-model. - * @return a boolean value. + * @return subModelBaseAcceleration the acceleration of the sub-model base. */ - bool getBaseAcceleration(manif::SE3d::Tangent& robotBaseAcceleration, - Eigen::Ref robotJointAcceleration, - manif::SE3d::Tangent& subModelBaseAcceleration); + const manif::SE3d::Tangent& getBaseAcceleration(); /** * @brief getBaseVelocity gets the acceleration of the sub-model base. - * @return baseVelocity the the base velocity of the sub-model. + * @return baseVelocity the velocity of the sub-model base. */ const manif::SE3d::Tangent& getBaseVelocity(); @@ -204,6 +213,13 @@ class SubModelKinDynWrapper * @return a boolean value saying if the rotation matrix is found. */ const manif::SO3d& getAccelerometerRotation(const std::string& accName) const; + + /** + * @brief getAccelerometerVelocity access the velocity of the accelerometer specified by the input param. + * @param accName is the name of the accelerometer. + * @return the velocity of the accelerometer. + */ + const manif::SE3d::Tangent& getAccelerometerVelocity(const std::string& accName); }; } // namespace RobotDynamicsEstimator diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h index 13d22722f9..8fa255077e 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h @@ -44,6 +44,9 @@ class UkfState : public bfl::AdditiveStateModel struct Impl; std::unique_ptr m_pimpl; +// void unpackState(); +// void updateJointAccelerationState(); + public: /** * Constructor. diff --git a/src/Estimators/src/AccelerometerMeasurementDynamics.cpp b/src/Estimators/src/AccelerometerMeasurementDynamics.cpp index c29ade2a98..403f802515 100644 --- a/src/Estimators/src/AccelerometerMeasurementDynamics.cpp +++ b/src/Estimators/src/AccelerometerMeasurementDynamics.cpp @@ -97,6 +97,12 @@ bool RDE::AccelerometerMeasurementDynamics::finalize(const System::VariablesHand return false; } + if (!m_isSubModelListSet) + { + log()->error("{} Please call `setSubModels` before finalizing.", errorPrefix); + return false; + } + if (stateVariableHandler.getNumberOfVariables() == 0) { log()->error("{} The state variable handler is empty.", errorPrefix); @@ -112,39 +118,27 @@ bool RDE::AccelerometerMeasurementDynamics::finalize(const System::VariablesHand } // Search and save all the submodels containing the sensor - for (int submodelIndex = 0; submodelIndex < m_nrOfSubDynamics; submodelIndex++) + for (int submodelIndex = 0; submodelIndex < m_subModelList.size(); submodelIndex++) { - if (m_subDynamics[submodelIndex]->subModel.hasAccelerometer(m_name)) - { - m_subModelsWithAcc.push_back(submodelIndex); - } + if (m_subModelList[submodelIndex].hasAccelerometer(m_name)) + { + m_subModelsWithAccelerometer.push_back(submodelIndex); + } } - m_covariances.resize(m_covSingleVar.size() * m_subModelsWithAcc.size()); - for (int index = 0; index < m_subModelsWithAcc.size(); index++) + m_covariances.resize(m_covSingleVar.size() * m_subModelsWithAccelerometer.size()); + for (int index = 0; index < m_subModelsWithAccelerometer.size(); index++) { m_covariances.segment(index*m_covSingleVar.size(), m_covSingleVar.size()) = m_covSingleVar; } m_size = m_covariances.size(); - m_motorTorqueFullModel.resize(m_stateVariableHandler.getVariable("tau_m").size); - m_motorTorqueFullModel.setZero(); - - m_frictionTorqueFullModel.resize(m_stateVariableHandler.getVariable("tau_F").size); - m_frictionTorqueFullModel.setZero(); - - m_jointAccelerationFullModel.resize(m_stateVariableHandler.getVariable("ds").size); - m_jointAccelerationFullModel.setZero(); - - m_jointVelocityFullModel.resize(m_stateVariableHandler.getVariable("ds").size); - m_jointVelocityFullModel.setZero(); - - m_subModelJointAcc.resize(m_nrOfSubDynamics); + m_subModelJointAcc.resize(m_subModelList.size()); - for (int idx = 0; idx < m_nrOfSubDynamics; idx++) + for (int idx = 0; idx < m_subModelList.size(); idx++) { - m_subModelJointAcc[idx].resize(m_subDynamics[idx]->subModel.getJointMapping().size()); + m_subModelJointAcc[idx].resize(m_subModelList[idx].getJointMapping().size()); m_subModelJointAcc[idx].setZero(); } @@ -154,6 +148,8 @@ bool RDE::AccelerometerMeasurementDynamics::finalize(const System::VariablesHand m_updatedVariable.resize(m_size); m_updatedVariable.setZero(); + m_accFrameVel.setZero(); + return true; } @@ -161,36 +157,12 @@ bool RDE::AccelerometerMeasurementDynamics::setSubModels(const std::vector()); - - if(!m_subDynamics[subModelIdx]->setSubModel(subModelList[subModelIdx])) - { - log()->error("{} The submodel at index {} is not valid.", errorPrefix, subModelIdx); - return false; - } - - if(!m_subDynamics[subModelIdx]->setKinDynWrapper(kinDynWrapperList[subModelIdx])) - { - log()->error("{} The submodel kindyn wrapper at index {} is not valid.", errorPrefix, subModelIdx); - return false; - } - } + m_subModelList = subModelList; - m_nrOfSubDynamics = m_subDynamics.size(); + m_kinDynWrapperList = kinDynWrapperList; m_isSubModelListSet = true; - for (int subModelIdx = 0; subModelIdx < m_nrOfSubDynamics; subModelIdx++) - { - if (!m_subDynamics.at(subModelIdx)->initialize()) - { - log()->error("{} Cannot initialize the accelerometer measurement dynamics of the sub-model {}.", errorPrefix, subModelIdx); - return false; - } - } - return true; } @@ -233,22 +205,22 @@ bool RDE::AccelerometerMeasurementDynamics::checkStateVariableHandler() } // check if all the sensors are part of the sub-model - for (int subModelIdx = 0; subModelIdx < m_nrOfSubDynamics; subModelIdx++) + for (int subModelIdx = 0; subModelIdx < m_subModelList.size(); subModelIdx++) { - for (int ftIdx = 0; ftIdx < m_subDynamics.at(subModelIdx)->subModel.getNrOfFTSensor(); ftIdx++) + for (int ftIdx = 0; ftIdx < m_subModelList[subModelIdx].getNrOfFTSensor(); ftIdx++) { - if (!m_stateVariableHandler.getVariable(m_subDynamics.at(subModelIdx)->subModel.getFTSensor(ftIdx).name).isValid()) + if (!m_stateVariableHandler.getVariable(m_subModelList[subModelIdx].getFTSensor(ftIdx).name).isValid()) { - log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_subDynamics.at(subModelIdx)->subModel.getFTSensor(ftIdx).name); + log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_subModelList[subModelIdx].getFTSensor(ftIdx).name); return false; } } - for (int contactIdx = 0; contactIdx < m_subDynamics.at(subModelIdx)->subModel.getNrOfExternalContact(); contactIdx++) + for (int contactIdx = 0; contactIdx < m_subModelList[subModelIdx].getNrOfExternalContact(); contactIdx++) { - if (!m_stateVariableHandler.getVariable(m_subDynamics.at(subModelIdx)->subModel.getExternalContact(contactIdx)).isValid()) + if (!m_stateVariableHandler.getVariable(m_subModelList[subModelIdx].getExternalContact(contactIdx)).isValid()) { - log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_subDynamics.at(subModelIdx)->subModel.getExternalContact(contactIdx)); + log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_subModelList[subModelIdx].getExternalContact(contactIdx)); return false; } } @@ -262,44 +234,39 @@ bool RDE::AccelerometerMeasurementDynamics::update() constexpr auto errorPrefix = "[AccelerometerMeasurementDynamics::update]"; // compute joint acceleration per each sub-model containing the accelerometer - for (int subDynamicsIdx = 0; subDynamicsIdx < m_subDynamics.size(); subDynamicsIdx++) + for (int subDynamicsIdx = 0; subDynamicsIdx < m_subModelList.size(); subDynamicsIdx++) { - if (m_subDynamics[subDynamicsIdx]->subModel.getModel().getNrOfDOFs() > 0) + if (m_subModelList[subDynamicsIdx].getModel().getNrOfDOFs() > 0) { - if (!m_subDynamics[subDynamicsIdx]->update(m_ukfInput.robotBaseAcceleration, m_jointAccelerationFullModel, m_subModelJointAcc[subDynamicsIdx])) - { - log()->error("{} Error updating the joint velocity dynamics for the sub-model {}.", errorPrefix, subDynamicsIdx); - return false; - } - - for (int jointIdx = 0; jointIdx < m_subDynamics[subDynamicsIdx]->subModel.getJointMapping().size(); jointIdx++) + for (int jointIdx = 0; jointIdx < m_subModelList[subDynamicsIdx].getJointMapping().size(); jointIdx++) { - m_jointAccelerationFullModel[m_subDynamics[subDynamicsIdx]->subModel.getJointMapping()[jointIdx]] = m_subModelJointAcc[subDynamicsIdx][jointIdx]; + m_subModelJointAcc[subDynamicsIdx][jointIdx] = m_ukfInput.robotJointAccelerations[m_subModelList[subDynamicsIdx].getJointMapping()[jointIdx]]; } } } // J_dot nu + base_J v_dot_base + joint_J s_dotdot - acc_Rot_world gravity + bias - for (int index = 0; index < m_subModelsWithAcc.size(); index++) + for (int index = 0; index < m_subModelsWithAccelerometer.size(); index++) { - m_JdotNu = m_subDynamics[m_subModelsWithAcc[index]]->kinDynWrapper->getAccelerometerBiasAcceleration(m_name); + m_JdotNu = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerBiasAcceleration(m_name); - if (!m_subDynamics[m_subModelsWithAcc[index]]->kinDynWrapper->getBaseAcceleration(m_ukfInput.robotBaseAcceleration, m_jointAccelerationFullModel, m_subModelBaseAcc)) - { - log()->error("{} Error getting the sub-model base acceleration.", errorPrefix); - return false; - } + m_JvdotBase = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerJacobian(m_name).block(0, 0, 6, 6) * + m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getBaseAcceleration().coeffs(); - m_JvdotBase = m_subDynamics[m_subModelsWithAcc[index]]->kinDynWrapper->getAccelerometerJacobian(m_name).block(0, 0, 6, 6) * m_subModelBaseAcc.coeffs(); + m_accRg = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerRotation(m_name).rotation() * m_gravity; - m_accRg = m_subDynamics[m_subModelsWithAcc[index]]->kinDynWrapper->getAccelerometerRotation(m_name).rotation() * m_gravity; + linVel = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerVelocity(m_name).coeffs().segment(0, 3); + angVel = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerVelocity(m_name).coeffs().segment(3, 6); - m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) = m_JdotNu.segment(0, 3) + m_JvdotBase.segment(0, 3) - m_accRg + m_bias; + m_vCrossW = linVel.cross(angVel); - if (m_subDynamics[m_subModelsWithAcc[index]]->subModel.getJointMapping().size() > 0) + m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) = m_JdotNu.segment(0, 3) + m_JvdotBase.segment(0, 3) - m_vCrossW - m_accRg + m_bias; + + if (m_subModelList[m_subModelsWithAccelerometer[index]].getJointMapping().size() > 0) { - m_Jsdotdot = m_subDynamics[m_subModelsWithAcc[index]]->kinDynWrapper->getAccelerometerJacobian(m_name).block(0, 6, 6, m_subModelJointAcc[m_subModelsWithAcc[index]].size()) * - m_subModelJointAcc[m_subModelsWithAcc[index]]; + m_Jsdotdot = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerJacobian(m_name). + block(0, 6, 6, m_subModelJointAcc[m_subModelsWithAccelerometer[index]].size()) * + m_subModelJointAcc[m_subModelsWithAccelerometer[index]]; m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) += m_Jsdotdot.segment(0, 3); } @@ -310,26 +277,8 @@ bool RDE::AccelerometerMeasurementDynamics::update() void RDE::AccelerometerMeasurementDynamics::setState(const Eigen::Ref ukfState) { - m_jointVelocityFullModel = ukfState.segment(m_stateVariableHandler.getVariable("ds").offset, - m_stateVariableHandler.getVariable("ds").size); - - m_motorTorqueFullModel = ukfState.segment(m_stateVariableHandler.getVariable("tau_m").offset, - m_stateVariableHandler.getVariable("tau_m").size); - - m_frictionTorqueFullModel = ukfState.segment(m_stateVariableHandler.getVariable("tau_F").offset, - m_stateVariableHandler.getVariable("tau_F").size); - m_bias = ukfState.segment(m_stateVariableHandler.getVariable(m_biasVariableName).offset, m_stateVariableHandler.getVariable(m_biasVariableName).size); - - for (int subDynamicsIdx = 0; subDynamicsIdx < m_subDynamics.size(); subDynamicsIdx++) - { - m_subDynamics.at(subDynamicsIdx)->setState(ukfState, - m_jointVelocityFullModel, - m_motorTorqueFullModel, - m_frictionTorqueFullModel, - m_stateVariableHandler); - } } void RDE::AccelerometerMeasurementDynamics::setInput(const UKFInput& ukfInput) diff --git a/src/Estimators/src/FrictionTorqueStateDynamics.cpp b/src/Estimators/src/FrictionTorqueStateDynamics.cpp index 29719404f3..ed5516fbe1 100644 --- a/src/Estimators/src/FrictionTorqueStateDynamics.cpp +++ b/src/Estimators/src/FrictionTorqueStateDynamics.cpp @@ -120,44 +120,24 @@ bool RDE::FrictionTorqueStateDynamics::finalize(const System::VariablesHandler & m_size = m_covariances.size(); - m_motorTorqueFullModel.resize(m_stateVariableHandler.getVariable("tau_m").size); - m_motorTorqueFullModel.setZero(); - m_frictionTorqueFullModel.resize(m_stateVariableHandler.getVariable("tau_F").size); m_frictionTorqueFullModel.setZero(); - m_jointAccelerationFullModel.resize(m_stateVariableHandler.getVariable("ds").size); - m_jointAccelerationFullModel.setZero(); - m_jointVelocityFullModel.resize(m_stateVariableHandler.getVariable("ds").size); m_jointVelocityFullModel.setZero(); - m_subModelUpdatedJointAcceleration.resize(m_nrOfSubDynamics); - - for (int idx = 0; idx < m_nrOfSubDynamics; idx++) - { - m_subModelUpdatedJointAcceleration[idx].resize(m_subDynamics[idx]->subModel.getJointMapping().size()); - m_subModelUpdatedJointAcceleration[idx].setZero(); - } - - m_currentFrictionTorque.resize(m_size); - m_currentFrictionTorque.setZero(); - m_updatedVariable.resize(m_size); m_updatedVariable.setZero(); - m_tanhArgument.resize(m_size); - m_tanhArgument.setZero(); + m_coshArgument.resize(m_size); + m_coshArgument.setZero(); - m_tanh.resize(m_size); - m_tanh.setZero(); + m_coshsquared.resize(m_size); + m_coshsquared.setZero(); m_k0k1.resize(m_size); m_k0k1.setZero(); - m_argParenthesis.resize(m_size); - m_argParenthesis.setZero(); - m_dotTauF.resize(m_size); m_dotTauF.setZero(); @@ -166,41 +146,6 @@ bool RDE::FrictionTorqueStateDynamics::finalize(const System::VariablesHandler & bool RDE::FrictionTorqueStateDynamics::setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) { - constexpr auto errorPrefix = "[FrictionTorqueStateDynamics::setSubModels]"; - - for (int subModelIdx = 0; subModelIdx < subModelList.size(); subModelIdx++) - { - if (subModelList[subModelIdx].getModel().getNrOfDOFs() > 0) - { - m_subDynamics.emplace_back(std::make_unique()); - - if(!m_subDynamics[subModelIdx]->setSubModel(subModelList[subModelIdx])) - { - log()->error("{} The submodel at index {} is not valid.", errorPrefix, subModelIdx); - return false; - } - - if(!m_subDynamics[subModelIdx]->setKinDynWrapper(kinDynWrapperList[subModelIdx])) - { - log()->error("{} The submodel kindyn wrapper at index {} is not valid.", errorPrefix, subModelIdx); - return false; - } - } - } - - m_nrOfSubDynamics = m_subDynamics.size(); - - m_isSubModelListSet = true; - - for (int subModelIdx = 0; subModelIdx < m_nrOfSubDynamics; subModelIdx++) - { - if (!m_subDynamics.at(subModelIdx)->initialize()) - { - log()->error("{} Cannot initialize the joint velocity dynamics of the sub-model {}.", errorPrefix, subModelIdx); - return false; - } - } - return true; } @@ -208,12 +153,6 @@ bool RDE::FrictionTorqueStateDynamics::checkStateVariableHandler() { constexpr auto errorPrefix = "[FrictionTorqueStateDynamics::checkStateVariableHandler]"; - if (!m_isSubModelListSet) - { - log()->error("{} Set the sub-model list before setting the variable handler", errorPrefix); - return false; - } - if (!m_stateVariableHandler.getVariable("tau_m").isValid()) { log()->error("{} The variable handler does not contain the expected state with name `tau_m`.", errorPrefix); @@ -233,28 +172,6 @@ bool RDE::FrictionTorqueStateDynamics::checkStateVariableHandler() return false; } - // check if all the sensors are part of the sub-model - for (int subModelIdx = 0; subModelIdx < m_nrOfSubDynamics; subModelIdx++) - { - for (int ftIdx = 0; ftIdx < m_subDynamics.at(subModelIdx)->subModel.getNrOfFTSensor(); ftIdx++) - { - if (!m_stateVariableHandler.getVariable(m_subDynamics.at(subModelIdx)->subModel.getFTSensor(ftIdx).name).isValid()) - { - log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_subDynamics.at(subModelIdx)->subModel.getFTSensor(ftIdx).name); - return false; - } - } - - for (int contactIdx = 0; contactIdx < m_subDynamics.at(subModelIdx)->subModel.getNrOfExternalContact(); contactIdx++) - { - if (!m_stateVariableHandler.getVariable(m_subDynamics.at(subModelIdx)->subModel.getExternalContact(contactIdx)).isValid()) - { - log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_subDynamics.at(subModelIdx)->subModel.getExternalContact(contactIdx)); - return false; - } - } - } - return true; } @@ -262,40 +179,20 @@ bool RDE::FrictionTorqueStateDynamics::checkStateVariableHandler() // Change the model bool RDE::FrictionTorqueStateDynamics::update() { - constexpr auto errorPrefix = "[FrictionTorqueStateDynamics::update]"; - - // compute joint acceleration per each sub-model - for (int subDynamicsIdx = 0; subDynamicsIdx < m_subDynamics.size(); subDynamicsIdx++) - { - if (!m_subDynamics[subDynamicsIdx]->update(m_ukfInput.robotBaseAcceleration, m_jointAccelerationFullModel, m_subModelUpdatedJointAcceleration[subDynamicsIdx])) - { - log()->error("{} Error updating the joint velocity dynamics for the sub-model {}.", errorPrefix, subDynamicsIdx); - return false; - } - - for (int jointIdx = 0; jointIdx < m_subDynamics[subDynamicsIdx]->subModel.getJointMapping().size(); jointIdx++) - { - m_jointAccelerationFullModel[m_subDynamics[subDynamicsIdx]->subModel.getJointMapping()[jointIdx]] = m_subModelUpdatedJointAcceleration[subDynamicsIdx][jointIdx]; - } - } - // k_{1} \dot{s,k} - m_tanhArgument = m_k1.array() * m_jointVelocityFullModel.array(); + m_coshArgument = m_k1.array() * m_jointVelocityFullModel.array(); // tanh (k_{1} \dot{s,k})) - m_tanh = m_tanhArgument.array().tanh(); - - // 1 - tanh^{2} (k_{1} \dot{s,k}) - m_argParenthesis = 1 - m_tanh.array().square(); + m_coshsquared = m_coshArgument.array().cosh().square(); // k_{0} k_{1} m_k0k1 = m_k0.array() * m_k1.array(); // \ddot{s,k} ( k_{2} + k_{0} k_{1} (1 - tanh^{2} (k_{1} \dot{s,k})) ) - m_dotTauF = m_jointAccelerationFullModel.array() * ( m_k2.array() + m_k0k1.array() * m_argParenthesis.array() ); + m_dotTauF = m_ukfInput.robotJointAccelerations.array() * ( m_k2.array() + m_k0k1.array() / m_coshsquared.array() ); // \tau_{F,k+1} = \tau_{F,k} + \Delta T * \dot{\tau_{F,k}} - m_updatedVariable = m_currentFrictionTorque + m_dT * m_dotTauF; + m_updatedVariable = m_frictionTorqueFullModel + m_dT * m_dotTauF; return true; } @@ -305,20 +202,8 @@ void RDE::FrictionTorqueStateDynamics::setState(const Eigen::RefsetState(ukfState, - m_jointVelocityFullModel, - m_motorTorqueFullModel, - m_frictionTorqueFullModel, - m_stateVariableHandler); - } } void RDE::FrictionTorqueStateDynamics::setInput(const UKFInput& ukfInput) diff --git a/src/Estimators/src/JointVelocityStateDynamics.cpp b/src/Estimators/src/JointVelocityStateDynamics.cpp index 7963695444..c9f25a4328 100644 --- a/src/Estimators/src/JointVelocityStateDynamics.cpp +++ b/src/Estimators/src/JointVelocityStateDynamics.cpp @@ -15,43 +15,8 @@ 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) { - constexpr auto errorPrefix = "[JointVelocityStateDynamics::setSubModels]"; - - for (int subModelIdx = 0; subModelIdx < subModelList.size(); subModelIdx++) - { - if (subModelList[subModelIdx].getModel().getNrOfDOFs() > 0) - { - m_subDynamics.emplace_back(std::make_unique()); - - if(!m_subDynamics[subModelIdx]->setSubModel(subModelList[subModelIdx])) - { - log()->error("{} The submodel at index {} is not valid.", errorPrefix, subModelIdx); - return false; - } - - if(!m_subDynamics[subModelIdx]->setKinDynWrapper(kinDynWrapperList[subModelIdx])) - { - log()->error("{} The submodel kindyn wrapper at index {} is not valid.", errorPrefix, subModelIdx); - return false; - } - } - } - - m_nrOfSubDynamics = m_subDynamics.size(); - - m_isSubModelListSet = true; - - for (int subModelIdx = 0; subModelIdx < m_nrOfSubDynamics; subModelIdx++) - { - if (!m_subDynamics.at(subModelIdx)->initialize()) - { - log()->error("{} Cannot initialize the joint velocity dynamics of the sub-model {}.", errorPrefix, subModelIdx); - return false; - } - } - return true; } @@ -59,12 +24,6 @@ bool RDE::JointVelocityStateDynamics::checkStateVariableHandler() { constexpr auto errorPrefix = "[JointVelocityStateDynamics::checkStateVariableHandler]"; - if (!m_isSubModelListSet) - { - log()->error("{} Set the sub-model list before setting the variable handler", errorPrefix); - return false; - } - // Check if the variable handler contains the variables used by this dynamics if (!m_stateVariableHandler.getVariable("tau_m").isValid()) { @@ -78,27 +37,6 @@ bool RDE::JointVelocityStateDynamics::checkStateVariableHandler() return false; } - for (int subModelIdx = 0; subModelIdx < m_nrOfSubDynamics; subModelIdx++) - { - for (int ftIdx = 0; ftIdx < m_subDynamics.at(subModelIdx)->subModel.getNrOfFTSensor(); ftIdx++) - { - if (!m_stateVariableHandler.getVariable(m_subDynamics.at(subModelIdx)->subModel.getFTSensor(ftIdx).name).isValid()) - { - log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_subDynamics.at(subModelIdx)->subModel.getFTSensor(ftIdx).name); - return false; - } - } - - for (int contactIdx = 0; contactIdx < m_subDynamics.at(subModelIdx)->subModel.getNrOfExternalContact(); contactIdx++) - { - if (!m_stateVariableHandler.getVariable(m_subDynamics.at(subModelIdx)->subModel.getExternalContact(contactIdx)).isValid()) - { - log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_subDynamics.at(subModelIdx)->subModel.getExternalContact(contactIdx)); - return false; - } - } - } - return true; } @@ -187,59 +125,19 @@ bool RDE::JointVelocityStateDynamics::finalize(const System::VariablesHandler &s m_size = m_covariances.size(); - m_motorTorqueFullModel.resize(m_stateVariableHandler.getVariable("tau_m").size); - m_motorTorqueFullModel.setZero(); - - m_frictionTorqueFullModel.resize(m_stateVariableHandler.getVariable("tau_F").size); - m_frictionTorqueFullModel.setZero(); - m_jointVelocityFullModel.resize(m_stateVariableHandler.getVariable("ds").size); m_jointVelocityFullModel.setZero(); - m_jointAccelerationFullModel.resize(m_stateVariableHandler.getVariable("ds").size); - m_jointAccelerationFullModel.setZero(); - m_updatedVariable.resize(m_stateVariableHandler.getVariable("ds").size); m_updatedVariable.setZero(); - m_subModelUpdatedJointVelocity.resize(m_nrOfSubDynamics); - m_subModelUpdatedJointAcceleration.resize(m_nrOfSubDynamics); - - for (int idx = 0; idx < m_nrOfSubDynamics; idx++) - { - m_subModelUpdatedJointVelocity[idx].resize(m_subDynamics[idx]->subModel.getJointMapping().size()); - m_subModelUpdatedJointVelocity[idx].setZero(); - - m_subModelUpdatedJointAcceleration[idx].resize(m_subDynamics[idx]->subModel.getJointMapping().size()); - m_subModelUpdatedJointAcceleration[idx].setZero(); - } - return true; } bool RDE::JointVelocityStateDynamics::update() { - constexpr auto errorPrefix = "[JointVelocityStateDynamics::update]"; - - // compute joint acceleration per each sub-model - for (int subDynamicsIdx = 0; subDynamicsIdx < m_subDynamics.size(); subDynamicsIdx++) - { - if (!m_subDynamics[subDynamicsIdx]->update(m_ukfInput.robotBaseAcceleration, m_jointAccelerationFullModel, m_subModelUpdatedJointAcceleration[subDynamicsIdx])) - { - log()->error("{} Error updating the joint velocity dynamics for the sub-model {}.", errorPrefix, subDynamicsIdx); - return false; - } - - m_subModelUpdatedJointVelocity[subDynamicsIdx] = m_subModelUpdatedJointVelocity[subDynamicsIdx] + m_dT * m_subModelUpdatedJointAcceleration[subDynamicsIdx]; - - for (int jointIdx = 0; jointIdx < m_subDynamics[subDynamicsIdx]->subModel.getJointMapping().size(); jointIdx++) - { - m_jointAccelerationFullModel[m_subDynamics[subDynamicsIdx]->subModel.getJointMapping()[jointIdx]] = m_subModelUpdatedJointAcceleration[subDynamicsIdx][jointIdx]; - - m_updatedVariable[m_subDynamics[subDynamicsIdx]->subModel.getJointMapping()[jointIdx]] = m_subModelUpdatedJointVelocity[subDynamicsIdx][jointIdx]; - } - } + m_updatedVariable = m_jointVelocityFullModel + m_dT * m_ukfInput.robotJointAccelerations; return true; } @@ -248,21 +146,6 @@ void RDE::JointVelocityStateDynamics::setState(const Eigen::RefsetState(ukfState, - m_jointVelocityFullModel, - m_motorTorqueFullModel, - m_frictionTorqueFullModel, - m_stateVariableHandler); - } } void RDE::JointVelocityStateDynamics::setInput(const UKFInput& ukfInput) diff --git a/src/Estimators/src/SubModelDynamics.cpp b/src/Estimators/src/SubModelDynamics.cpp index adbbe55fb3..eb6cb154c1 100644 --- a/src/Estimators/src/SubModelDynamics.cpp +++ b/src/Estimators/src/SubModelDynamics.cpp @@ -7,7 +7,6 @@ #include #include -#include #include #include @@ -119,8 +118,8 @@ void RDE::SubModelDynamics::setState(const Eigen::Ref ukf for (int idx = 0; idx < subModel.getNrOfExternalContact(); idx++) { - FTMap[subModel.getExternalContact(idx)] = ukfState.segment(variableHandler.getVariable(subModel.getExternalContact(idx)).offset, - variableHandler.getVariable(subModel.getExternalContact(idx)).size); + extContactMap[subModel.getExternalContact(idx)] = ukfState.segment(variableHandler.getVariable(subModel.getExternalContact(idx)).offset, + variableHandler.getVariable(subModel.getExternalContact(idx)).size); } } @@ -168,7 +167,7 @@ void RDE::SubModelDynamics::computeTotalTorqueFromContacts() // Contribution of unknown external contacts for (int idx = 0; idx < subModel.getNrOfExternalContact(); idx++) { - torqueFromContact = kinDynWrapper->getFTJacobian(subModel.getFTSensor(idx).name).block(0, 6, 6, subModel.getModel().getNrOfDOFs()).transpose() * FTMap[subModel.getExternalContact(idx)]; + torqueFromContact = kinDynWrapper->getExtContactJacobian(subModel.getExternalContact(idx)).block(0, 6, 6, subModel.getModel().getNrOfDOFs()).transpose() * extContactMap[subModel.getExternalContact(idx)]; totalTorqueFromContacts = totalTorqueFromContacts.array() + torqueFromContact.array(); } diff --git a/src/Estimators/src/SubModelKinDynWrapper.cpp b/src/Estimators/src/SubModelKinDynWrapper.cpp index 9cc2c08967..a0876de522 100644 --- a/src/Estimators/src/SubModelKinDynWrapper.cpp +++ b/src/Estimators/src/SubModelKinDynWrapper.cpp @@ -69,6 +69,8 @@ bool RDE::SubModelKinDynWrapper::initialize(const RDE::SubModel& subModel) m_FTBaseAcc.resize(m_numOfJoints); m_totalTorques.resize(m_numOfJoints); + m_subModelBaseAcceleration.setZero(); + // Initialize attributes with correct sizes m_massMatrix.resize(6 + m_subModel.getModel().getNrOfDOFs(), 6 + m_subModel.getModel().getNrOfDOFs()); @@ -97,6 +99,10 @@ bool RDE::SubModelKinDynWrapper::initialize(const RDE::SubModel& subModel) manif::SO3d rotMatrix; rotMatrix.coeffs().setZero(); m_accRworldList[m_subModel.getAccelerometerList()[idx].name] = std::move(rotMatrix); + + manif::SE3d::Tangent vel; + vel.coeffs().setZero(); + m_accVelList[m_subModel.getAccelerometerList()[idx].name] = std::move(vel); } for (int idx = 0; idx < m_subModel.getGyroscopeList().size(); idx++) @@ -116,7 +122,9 @@ bool RDE::SubModelKinDynWrapper::initialize(const RDE::SubModel& subModel) return true; } -bool RDE::SubModelKinDynWrapper::updateInternalKinDynState(bool isCorrectStep) +bool RDE::SubModelKinDynWrapper::updateState(manif::SE3d::Tangent& robotBaseAcceleration, + Eigen::Ref robotJointAcceleration, + bool isCorrectStep) { constexpr auto logPrefix = "[BipedalLocomotion::Estimators::SubModelKinDynWrapper::" "updateKinDynState]"; @@ -148,6 +156,19 @@ bool RDE::SubModelKinDynWrapper::updateInternalKinDynState(bool isCorrectStep) return false; } + if (!m_kinDynFullModel->getFrameAcc(m_baseFrame, + iDynTree::make_span(robotBaseAcceleration.data(), + manif::SE3d::Tangent::DoF), + robotJointAcceleration, + iDynTree::make_span(m_subModelBaseAcceleration.data(), + manif::SE3d::Tangent::DoF))) + { + blf::log()->error("{} Unable to get the acceleration of the frame {}.", + logPrefix, + m_baseFrame); + return false; + } + return updateDynamicsVariableState(isCorrectStep); } @@ -212,6 +233,10 @@ bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState(bool isCorrectStep) m_kinDyn.getWorldTransform(m_subModel.getAccelerometerList()[idx].frame) .getRotation() .inverse()); + + // Update accelerometer velocity + m_accVelList[m_subModel.getAccelerometerList()[idx].name] = iDynTree::toEigen( + m_kinDyn.getFrameVel(m_subModel.getAccelerometerList()[idx].frame)); } // Update gyroscope jacobians @@ -287,28 +312,9 @@ bool RDE::SubModelKinDynWrapper::forwardDynamics(Eigen::Ref mot return true; } -bool RDE::SubModelKinDynWrapper::getBaseAcceleration( - manif::SE3d::Tangent& robotBaseAcceleration, - Eigen::Ref robotJointAcceleration, - manif::SE3d::Tangent& subModelBaseAcceleration) +const manif::SE3d::Tangent& RDE::SubModelKinDynWrapper::getBaseAcceleration() { - constexpr auto logPrefix = "[BipedalLocomotion::Estimators::SubModelKinDynWrapper::" - "getBaseAcceleration]"; - - if (!m_kinDynFullModel->getFrameAcc(m_baseFrame, - iDynTree::make_span(robotBaseAcceleration.data(), - manif::SE3d::Tangent::DoF), - robotJointAcceleration, - iDynTree::make_span(subModelBaseAcceleration.data(), - manif::SE3d::Tangent::DoF))) - { - blf::log()->error("{} Unable to get the acceleration of the frame {}.", - logPrefix, - m_baseFrame); - return false; - } - - return true; + return m_subModelBaseAcceleration; } const manif::SE3d::Tangent& RDE::SubModelKinDynWrapper::getBaseVelocity() @@ -367,3 +373,9 @@ RDE::SubModelKinDynWrapper::getAccelerometerRotation(const std::string& accName) { return m_accRworldList.at(accName); } + +const manif::SE3d::Tangent& +RDE::SubModelKinDynWrapper::getAccelerometerVelocity(const std::string& accName) +{ + return m_accVelList[accName]; +} diff --git a/src/Estimators/src/UkfMeasurement.cpp b/src/Estimators/src/UkfMeasurement.cpp index c3b15b75e8..14ccbbf2bc 100644 --- a/src/Estimators/src/UkfMeasurement.cpp +++ b/src/Estimators/src/UkfMeasurement.cpp @@ -9,6 +9,7 @@ #include #include +#include #include #include @@ -16,6 +17,7 @@ #include namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; +using namespace BipedalLocomotion; using namespace std::chrono; @@ -45,17 +47,135 @@ struct RDE::UkfMeasurement::Impl std::shared_ptr ukfInputProvider; /**< Provider containing the updated robot state. */ UKFInput ukfInput; /**< Struct containing the inputs for the ukf populated by the ukfInputProvider. */ - Eigen::VectorXd jointVelocityState; /**< Joint velocity compute by the ukf. */ + Eigen::VectorXd jointVelocityState; /**< Joint velocity computed by the ukf. */ + Eigen::VectorXd jointAccelerationState; /**< Joint acceleration computed from forward dynamics which depends on the current ukf state. */ Eigen::VectorXd currentState; /**< State estimated in the previous step. */ Eigen::VectorXd tempPredictedMeas; Eigen::MatrixXd predictedMeasurement; /**< Vector containing the updated measurement. */ + std::vector subModelJointVel; /**< List of sub-model joint velocities. */ + std::vector subModelJointAcc; /**< List of sub-model joint accelerations. */ + std::vector subModelJointMotorTorque; /**< List of sub-model joint velocities. */ + std::vector subModelFrictionTorque; /**< List of sub-model joint velocities. */ + std::map FTMap; /**< The map contains names of the ft sensors and values of the wrench */ + std::map extContactMap; /**< The map contains names of the ft sensors and values of the wrench */ + manif::SE3d::Tangent tempSubModelBaseAcc; /**< Acceleration of the base of the sub-model. */ + + // Support variables + std::vector totalTorqueFromContacts; /**< Joint torques due to known and unknown contacts on the sub-model. */ + std::vector torqueFromContact; /**< Joint torques due to a specific contact. */ + Math::Wrenchd wrench; /**< Joint torques due to a specific contact. */ + + Eigen::VectorXd measurement; /**< Measurements coming from the sensors. */ std::map measurementMap; /**< Measurement map . */ int offsetMeasurement; /**< Offset used to fill the measurement vector. */ + + void unpackState() + { + jointVelocityState = currentState.segment(stateVariableHandler.getVariable("ds").offset, + stateVariableHandler.getVariable("ds").size); + + for (int subModelIdx = 0; subModelIdx < subModelList.size(); subModelIdx++) + { + // Take sub-model joint velocities, motor torques, friction torques, ft wrenches, ext contact wrenches + for (int jointIdx = 0; jointIdx < subModelList[subModelIdx].getModel().getNrOfDOFs(); jointIdx++) + { + subModelJointVel[subModelIdx](jointIdx) = + jointVelocityState(subModelList[subModelIdx].getJointMapping()[jointIdx]); + + subModelJointMotorTorque[subModelIdx](jointIdx) = + currentState[stateVariableHandler.getVariable("tau_m").offset + + subModelList[subModelIdx].getJointMapping()[jointIdx]]; + + subModelFrictionTorque[subModelIdx](jointIdx) = + currentState[stateVariableHandler.getVariable("tau_F").offset + + subModelList[subModelIdx].getJointMapping()[jointIdx]]; + } + + for (int idx = 0; idx < subModelList[subModelIdx].getNrOfFTSensor(); idx++) + { + FTMap[subModelList[subModelIdx].getFTSensor(idx).name] = + currentState.segment(stateVariableHandler.getVariable(subModelList[subModelIdx].getFTSensor(idx).name).offset, + stateVariableHandler.getVariable(subModelList[subModelIdx].getFTSensor(idx).name).size); + } + + for (int idx = 0; idx < subModelList[subModelIdx].getNrOfExternalContact(); idx++) + { + extContactMap[subModelList[subModelIdx].getExternalContact(idx)] = + currentState.segment(stateVariableHandler.getVariable(subModelList[subModelIdx].getExternalContact(idx)).offset, + stateVariableHandler.getVariable(subModelList[subModelIdx].getExternalContact(idx)).size); + } + } + } + + bool updateState() + { + // Update kindyn full model + kinDynFullModel->setRobotState(ukfInput.robotBasePose.transform(), + ukfInput.robotJointPositions, + iDynTree::make_span(ukfInput.robotBaseVelocity.data(), manif::SE3d::Tangent::DoF), + jointVelocityState, + gravity); + + // compute joint acceleration per each sub-model containing the accelerometer + for (int subModelIdx = 0; subModelIdx < subModelList.size(); subModelIdx++) + { + if (subModelList[subModelIdx].getModel().getNrOfDOFs() > 0) + { + // Update the kindyn wrapper object of the submodel + kinDynWrapperList[subModelIdx]->updateState(ukfInput.robotBaseAcceleration, + jointAccelerationState, + false); + + totalTorqueFromContacts[subModelIdx].setZero(); + + // Contribution of FT measurements + for (int idx = 0; idx < subModelList[subModelIdx].getNrOfFTSensor(); idx++) + { + wrench = (int)subModelList[subModelIdx].getFTSensor(idx).forceDirection * + FTMap[subModelList[subModelIdx].getFTSensor(idx).name].array(); + + torqueFromContact[subModelIdx] = kinDynWrapperList[subModelIdx]-> + getFTJacobian(subModelList[subModelIdx].getFTSensor(idx).name). + block(0, 6, 6, subModelList[subModelIdx].getModel().getNrOfDOFs()).transpose() * wrench; + + totalTorqueFromContacts[subModelIdx] = totalTorqueFromContacts[subModelIdx].array() + torqueFromContact[subModelIdx].array(); + } + + // Contribution of unknown external contacts + for (int idx = 0; idx < subModelList[subModelIdx].getNrOfExternalContact(); idx++) + { + torqueFromContact[subModelIdx] = kinDynWrapperList[subModelIdx]->getExtContactJacobian(subModelList[subModelIdx].getExternalContact(idx)).block(0, 6, 6, subModelList[subModelIdx].getModel().getNrOfDOFs()).transpose() * extContactMap[subModelList[subModelIdx].getExternalContact(idx)]; + + totalTorqueFromContacts[subModelIdx] = totalTorqueFromContacts[subModelIdx].array() + torqueFromContact[subModelIdx].array(); + } + + tempSubModelBaseAcc = kinDynWrapperList[subModelIdx]->getBaseAcceleration(); + + if (!kinDynWrapperList[subModelIdx]->forwardDynamics(subModelJointMotorTorque[subModelIdx], + subModelFrictionTorque[subModelIdx], + totalTorqueFromContacts[subModelIdx], + tempSubModelBaseAcc.coeffs(), + subModelJointAcc[subModelIdx])) + { + BipedalLocomotion::log()->error("Cannot compute the inverse dynamics."); + return false; + } + + // Assign joint acceleration using the correct indeces + for (int jointIdx = 0; jointIdx < subModelList[subModelIdx].getJointMapping().size(); jointIdx++) + { + jointAccelerationState[subModelList[subModelIdx].getJointMapping()[jointIdx]] = subModelJointAcc[subModelIdx][jointIdx]; + } + } + } + + return true; + } }; RDE::UkfMeasurement::UkfMeasurement() @@ -125,6 +245,17 @@ bool RDE::UkfMeasurement::finalize(const System::VariablesHandler& handler) } m_pimpl->jointVelocityState.resize(m_pimpl->kinDynFullModel->model().getNrOfDOFs()); + m_pimpl->jointAccelerationState.resize(m_pimpl->kinDynFullModel->model().getNrOfDOFs()); + + for (int idx = 0; idx < m_pimpl->subModelList.size(); idx++) + { + m_pimpl->subModelJointVel.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + m_pimpl->subModelJointAcc.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + m_pimpl->subModelJointMotorTorque.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + m_pimpl->subModelFrictionTorque.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + m_pimpl->totalTorqueFromContacts.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + m_pimpl->torqueFromContact.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + } m_pimpl->currentState.resize(m_pimpl->measurementSize); m_pimpl->currentState.setZero(); @@ -308,8 +439,7 @@ std::pair RDE::UkfMeasurement::predictedMeasure(const Eigen::Re { m_pimpl->currentState = cur_states.block(0, index, cur_states.rows(), 1);; - m_pimpl->jointVelocityState = m_pimpl->currentState.segment(m_pimpl->stateVariableHandler.getVariable("ds").offset, - m_pimpl->stateVariableHandler.getVariable("ds").size); + m_pimpl->unpackState(); // Update kindyn full model m_pimpl->kinDynFullModel->setRobotState(m_pimpl->ukfInput.robotBasePose.transform(), @@ -318,12 +448,16 @@ std::pair RDE::UkfMeasurement::predictedMeasure(const Eigen::Re m_pimpl->jointVelocityState, m_pimpl->gravity); - // Update kindyn sub-models - for (int subModelIdx = 0; subModelIdx < m_pimpl->subModelList.size(); subModelIdx++) + m_pimpl->unpackState(); + + if (!m_pimpl->updateState()) { - m_pimpl->kinDynWrapperList[subModelIdx]->updateInternalKinDynState(true); + BipedalLocomotion::log()->error("{} The joint accelerations are not updated.", logPrefix); + throw std::runtime_error("Error"); } + m_pimpl->ukfInput.robotJointAccelerations = m_pimpl->jointAccelerationState; + // TODO // This could be parallelized diff --git a/src/Estimators/src/UkfState.cpp b/src/Estimators/src/UkfState.cpp index c705429739..d2a08e1ee3 100644 --- a/src/Estimators/src/UkfState.cpp +++ b/src/Estimators/src/UkfState.cpp @@ -6,7 +6,6 @@ */ #include -#include #include #include @@ -46,9 +45,126 @@ struct RDE::UkfState::Impl std::shared_ptr ukfInputProvider; /**< Provider containing the updated robot state. */ UKFInput ukfInput; /**< Struct containing the inputs for the ukf populated by the ukfInputProvider. */ - Eigen::VectorXd jointVelocityState; /**< Joint velocity compute by the ukf. */ + Eigen::VectorXd jointVelocityState; /**< Joint velocity computed by the ukf. */ + Eigen::VectorXd jointAccelerationState; /**< Joint acceleration computed from forward dynamics which depends on the current ukf state. */ Eigen::VectorXd currentState; /**< State estimated in the previous step. */ Eigen::VectorXd nextState; /**< Vector containing all the updated states. */ + + std::vector subModelJointVel; /**< List of sub-model joint velocities. */ + std::vector subModelJointAcc; /**< List of sub-model joint accelerations. */ + std::vector subModelJointMotorTorque; /**< List of sub-model joint motor torques. */ + std::vector subModelFrictionTorque; /**< List of sub-model friction torques. */ + std::map FTMap; /**< The map contains names of the ft sensors and values of the wrench */ + std::map extContactMap; /**< The map contains names of the ft sensors and values of the wrench */ + manif::SE3d::Tangent tempSubModelBaseAcc; /**< Acceleration of the base of the sub-model. */ + + // Support variables + std::vector totalTorqueFromContacts; /**< Joint torques due to known and unknown contacts on the sub-model. */ + std::vector torqueFromContact; /**< Joint torques due to a specific contact. */ + Math::Wrenchd wrench; /**< Joint torques due to a specific contact. */ + + void unpackState() + { + jointVelocityState = currentState.segment(stateVariableHandler.getVariable("ds").offset, + stateVariableHandler.getVariable("ds").size); + + for (int subModelIdx = 0; subModelIdx < subModelList.size(); subModelIdx++) + { + // Take sub-model joint velocities, motor torques, friction torques, ft wrenches, ext contact wrenches + for (int jointIdx = 0; jointIdx < subModelList[subModelIdx].getModel().getNrOfDOFs(); jointIdx++) + { + subModelJointVel[subModelIdx](jointIdx) = + jointVelocityState(subModelList[subModelIdx].getJointMapping()[jointIdx]); + + subModelJointMotorTorque[subModelIdx](jointIdx) = + currentState[stateVariableHandler.getVariable("tau_m").offset + + subModelList[subModelIdx].getJointMapping()[jointIdx]]; + + subModelFrictionTorque[subModelIdx](jointIdx) = + currentState[stateVariableHandler.getVariable("tau_F").offset + + subModelList[subModelIdx].getJointMapping()[jointIdx]]; + } + + for (int idx = 0; idx < subModelList[subModelIdx].getNrOfFTSensor(); idx++) + { + FTMap[subModelList[subModelIdx].getFTSensor(idx).name] = + currentState.segment(stateVariableHandler.getVariable(subModelList[subModelIdx].getFTSensor(idx).name).offset, + stateVariableHandler.getVariable(subModelList[subModelIdx].getFTSensor(idx).name).size); + } + + for (int idx = 0; idx < subModelList[subModelIdx].getNrOfExternalContact(); idx++) + { + extContactMap[subModelList[subModelIdx].getExternalContact(idx)] = + currentState.segment(stateVariableHandler.getVariable(subModelList[subModelIdx].getExternalContact(idx)).offset, + stateVariableHandler.getVariable(subModelList[subModelIdx].getExternalContact(idx)).size); + } + } + } + + bool updateState() + { + // Update kindyn full model + kinDynFullModel->setRobotState(ukfInput.robotBasePose.transform(), + ukfInput.robotJointPositions, + iDynTree::make_span(ukfInput.robotBaseVelocity.data(), manif::SE3d::Tangent::DoF), + jointVelocityState, + gravity); + + // compute joint acceleration per each sub-model containing the accelerometer + for (int subModelIdx = 0; subModelIdx < subModelList.size(); subModelIdx++) + { + if (subModelList[subModelIdx].getModel().getNrOfDOFs() > 0) + { + // Update the kindyn wrapper object of the submodel + kinDynWrapperList[subModelIdx]->updateState(ukfInput.robotBaseAcceleration, + jointAccelerationState, + false); + + totalTorqueFromContacts[subModelIdx].setZero(); + + // Contribution of FT measurements + for (int idx = 0; idx < subModelList[subModelIdx].getNrOfFTSensor(); idx++) + { + wrench = (int)subModelList[subModelIdx].getFTSensor(idx).forceDirection * + FTMap[subModelList[subModelIdx].getFTSensor(idx).name].array(); + + torqueFromContact[subModelIdx] = kinDynWrapperList[subModelIdx]-> + getFTJacobian(subModelList[subModelIdx].getFTSensor(idx).name). + block(0, 6, 6, subModelList[subModelIdx].getModel().getNrOfDOFs()).transpose() * wrench; + + totalTorqueFromContacts[subModelIdx] = totalTorqueFromContacts[subModelIdx].array() + torqueFromContact[subModelIdx].array(); + } + + // Contribution of unknown external contacts + for (int idx = 0; idx < subModelList[subModelIdx].getNrOfExternalContact(); idx++) + { + torqueFromContact[subModelIdx] = kinDynWrapperList[subModelIdx]->getExtContactJacobian(subModelList[subModelIdx].getExternalContact(idx)).block(0, 6, 6, subModelList[subModelIdx].getModel().getNrOfDOFs()).transpose() * extContactMap[subModelList[subModelIdx].getExternalContact(idx)]; + + totalTorqueFromContacts[subModelIdx] = totalTorqueFromContacts[subModelIdx].array() + torqueFromContact[subModelIdx].array(); + } + + tempSubModelBaseAcc = kinDynWrapperList[subModelIdx]->getBaseAcceleration(); + + if (!kinDynWrapperList[subModelIdx]->forwardDynamics(subModelJointMotorTorque[subModelIdx], + subModelFrictionTorque[subModelIdx], + totalTorqueFromContacts[subModelIdx], + tempSubModelBaseAcc.coeffs(), + subModelJointAcc[subModelIdx])) + { + log()->error("Cannot compute the inverse dynamics."); + return false; + } + + // Assign joint acceleration using the correct indeces + for (int jointIdx = 0; jointIdx < subModelList[subModelIdx].getJointMapping().size(); jointIdx++) + { + jointAccelerationState[subModelList[subModelIdx].getJointMapping()[jointIdx]] = subModelJointAcc[subModelIdx][jointIdx]; + } + } + } + + return true; + } }; RDE::UkfState::UkfState() @@ -125,6 +241,17 @@ bool RDE::UkfState::finalize(const System::VariablesHandler& handler) } m_pimpl->jointVelocityState.resize(m_pimpl->kinDynFullModel->model().getNrOfDOFs()); + m_pimpl->jointAccelerationState.resize(m_pimpl->kinDynFullModel->model().getNrOfDOFs()); + + for (int idx = 0; idx < m_pimpl->subModelList.size(); idx++) + { + m_pimpl->subModelJointVel.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + m_pimpl->subModelJointAcc.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + m_pimpl->subModelJointMotorTorque.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + m_pimpl->subModelFrictionTorque.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + m_pimpl->totalTorqueFromContacts.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + m_pimpl->torqueFromContact.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + } m_pimpl->currentState.resize(m_pimpl->stateSize); m_pimpl->currentState.setZero(); @@ -300,22 +427,16 @@ void RDE::UkfState::propagate(const Eigen::Ref& cur_state { m_pimpl->currentState = cur_states.block(0, index, cur_states.rows(), 1); - m_pimpl->jointVelocityState = m_pimpl->currentState.segment(m_pimpl->stateVariableHandler.getVariable("ds").offset, - m_pimpl->stateVariableHandler.getVariable("ds").size); + m_pimpl->unpackState(); - // Update kindyn full model - m_pimpl->kinDynFullModel->setRobotState(m_pimpl->ukfInput.robotBasePose.transform(), - m_pimpl->ukfInput.robotJointPositions, - iDynTree::make_span(m_pimpl->ukfInput.robotBaseVelocity.data(), manif::SE3d::Tangent::DoF), - m_pimpl->jointVelocityState, - m_pimpl->gravity); - - // Update kindyn sub-models - for (int subModelIdx = 0; subModelIdx < m_pimpl->subModelList.size(); subModelIdx++) + if (!m_pimpl->updateState()) { - m_pimpl->kinDynWrapperList[subModelIdx]->updateInternalKinDynState(false); + log()->error("{} The joint accelerations are not updated.", logPrefix); + throw std::runtime_error("Error"); } + m_pimpl->ukfInput.robotJointAccelerations = m_pimpl->jointAccelerationState; + // TODO // This could be parallelized diff --git a/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp index 341d540c40..59387799fe 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp @@ -160,7 +160,6 @@ TEST_CASE("Friction Torque Dynamics") kinDynWrapperList.emplace_back(std::make_shared()); REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); - REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState(true)); } AccelerometerMeasurementDynamics accDynamics; @@ -218,6 +217,15 @@ TEST_CASE("Friction Torque Dynamics") input.robotBaseVelocity = baseVelocity; input.robotBaseAcceleration = baseAcceleration; + input.robotJointAccelerations = Eigen::VectorXd(kinDyn->model().getNrOfDOFs()).setZero(); + + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + REQUIRE(kinDynWrapperList.at(idx)->updateState(baseAcceleration, + input.robotJointAccelerations, + true)); + } + accDynamics.setInput(input); accDynamics.setState(state); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp index bec23114e3..344324626f 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp @@ -151,12 +151,16 @@ TEST_CASE("Friction Torque Dynamics") std::vector> kinDynWrapperList; const auto & subModelList = subModelCreator.getSubModelList(); + manif::SE3d::Tangent baseVelocity, baseAcceleration; + baseVelocity.setZero(); + baseAcceleration.setZero(); + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) { kinDynWrapperList.emplace_back(std::make_shared()); REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); - REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState(true)); + REQUIRE(kinDynWrapperList.at(idx)->updateState(baseAcceleration, Eigen::VectorXd(subModelList[idx].getModel().getNrOfDOFs()).setZero(), true)); } FrictionTorqueStateDynamics tau_F_dynamics; @@ -194,22 +198,6 @@ TEST_CASE("Friction Torque Dynamics") state[offset+jointIndex] = jointTorques.jointTorques()[jointIndex]; } - tau_F_dynamics.setState(state); - REQUIRE(tau_F_dynamics.update()); - for (int idx = 0; idx < tau_F_dynamics.getUpdatedVariable().size(); idx++) - { - REQUIRE(std::abs(tau_F_dynamics.getUpdatedVariable()(idx)) < 0.1); - } - - // Set random friction torque - Eigen::VectorXd currentStateTauF(covariance.size()); - for (int index = 0; index < currentStateTauF.size(); index++) - { - currentStateTauF(index) = GENERATE(take(1, random(-30, 30))); - } - - state.segment(variableHandler.getVariable("tau_F").offset, variableHandler.getVariable("tau_F").size) = currentStateTauF; - // Create an input for the ukf state UKFInput input; @@ -225,12 +213,41 @@ TEST_CASE("Friction Torque Dynamics") input.robotBasePose = basePose; // Define base velocity and acceleration - manif::SE3d::Tangent baseVelocity, baseAcceleration; - baseVelocity.setZero(); - baseAcceleration.setZero(); input.robotBaseVelocity = baseVelocity; input.robotBaseAcceleration = baseAcceleration; + input.robotJointAccelerations = Eigen::VectorXd(kinDyn->model().getNrOfDOFs()).setZero(); + + input.robotJointAccelerations = Eigen::VectorXd(kinDyn->model().getNrOfDOFs()).setZero(); + + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + REQUIRE(kinDynWrapperList.at(idx)->updateState(input.robotBaseAcceleration, input.robotJointAccelerations, true)); + } + + tau_F_dynamics.setInput(input); + tau_F_dynamics.setState(state); + + REQUIRE(tau_F_dynamics.update()); + for (int idx = 0; idx < tau_F_dynamics.getUpdatedVariable().size(); idx++) + { + REQUIRE(std::abs(tau_F_dynamics.getUpdatedVariable()(idx)) < 0.1); + } + + // Set random friction torque + Eigen::VectorXd currentStateTauF(covariance.size()); + for (int index = 0; index < currentStateTauF.size(); index++) + { + currentStateTauF(index) = GENERATE(take(1, random(-30, 30))); + } + + state.segment(variableHandler.getVariable("tau_F").offset, variableHandler.getVariable("tau_F").size) = currentStateTauF; + + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + REQUIRE(kinDynWrapperList.at(idx)->updateState(input.robotBaseAcceleration, input.robotJointAccelerations, true)); + } + tau_F_dynamics.setInput(input); tau_F_dynamics.setState(state); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp index 4b01f3a10a..f373c8a8b9 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp @@ -160,7 +160,6 @@ TEST_CASE("Gyroscope Measurement Dynamics") kinDynWrapperList.emplace_back(std::make_shared()); REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); - REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState(true)); } GyroscopeMeasurementDynamics gyroDynamics; @@ -218,6 +217,13 @@ TEST_CASE("Gyroscope Measurement Dynamics") input.robotBaseVelocity = baseVelocity; input.robotBaseAcceleration = baseAcceleration; + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + REQUIRE(kinDynWrapperList.at(idx)->updateState(baseAcceleration, + Eigen::VectorXd(subModelList[idx].getModel().getNrOfDOFs()).setZero(), + true)); + } + gyroDynamics.setInput(input); gyroDynamics.setState(state); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp index 6d0937f817..324a4ad2bc 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp @@ -198,15 +198,14 @@ TEST_CASE("Joint Velocity Dynamics Without FT") SubModelCreator subModelCreatorWithFT; createSubModels(modelLoader, kinDyn, handlerFromConfig, useFT, subModelCreatorWithFT); - std::vector> kinDynWrapperListWithFT; + std::vector> kinDynWrapperList; const auto & subModelListWithFT = subModelCreatorWithFT.getSubModelList(); for (int idx = 0; idx < subModelCreatorWithFT.getSubModelList().size(); idx++) { - kinDynWrapperListWithFT.emplace_back(std::make_shared()); - REQUIRE(kinDynWrapperListWithFT.at(idx)->setKinDyn(kinDyn)); - REQUIRE(kinDynWrapperListWithFT.at(idx)->initialize(subModelListWithFT[idx])); - REQUIRE(kinDynWrapperListWithFT.at(idx)->updateInternalKinDynState(true)); + kinDynWrapperList.emplace_back(std::make_shared()); + REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); + REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelListWithFT[idx])); } manif::SE3d::Tangent robotBaseAcceleration; @@ -230,7 +229,7 @@ TEST_CASE("Joint Velocity Dynamics Without FT") JointVelocityStateDynamics ds; model = "JointVelocityStateDynamics"; parameterHandler->setParameter("dynamic_model", model); - REQUIRE(ds.setSubModels(subModelListWithFT, kinDynWrapperListWithFT)); + REQUIRE(ds.setSubModels(subModelListWithFT, kinDynWrapperList)); REQUIRE(ds.initialize(parameterHandler)); REQUIRE(ds.finalize(variableHandler)); @@ -265,6 +264,14 @@ TEST_CASE("Joint Velocity Dynamics Without FT") baseAcceleration.setZero(); input.robotBaseVelocity = baseVelocity; input.robotBaseAcceleration = baseAcceleration; + input.robotJointAccelerations = Eigen::VectorXd(kinDyn->model().getNrOfDOFs()).setZero(); + + for (int idx = 0; idx < subModelCreatorWithFT.getSubModelList().size(); idx++) + { + REQUIRE(kinDynWrapperList.at(idx)->updateState(baseAcceleration, + input.robotJointAccelerations, + true)); + } ds.setInput(input); @@ -334,7 +341,6 @@ TEST_CASE("Joint Velocity Dynamics With FT") kinDynWrapperListWithFT.emplace_back(std::make_shared()); REQUIRE(kinDynWrapperListWithFT.at(idx)->setKinDyn(kinDyn)); REQUIRE(kinDynWrapperListWithFT.at(idx)->initialize(subModelListWithFT[idx])); - REQUIRE(kinDynWrapperListWithFT.at(idx)->updateInternalKinDynState(true)); } manif::SE3d::Tangent robotBaseAcceleration; @@ -397,6 +403,15 @@ TEST_CASE("Joint Velocity Dynamics With FT") input.robotBaseVelocity = baseVelocity; input.robotBaseAcceleration = baseAcceleration; + for (int idx = 0; idx < subModelCreatorWithFT.getSubModelList().size(); idx++) + { + REQUIRE(kinDynWrapperListWithFT.at(idx)->updateState(baseAcceleration, + Eigen::VectorXd(kinDyn->model().getNrOfDOFs()).setZero(), + true)); + } + + input.robotJointAccelerations = Eigen::VectorXd(kinDyn->model().getNrOfDOFs()).setZero(); + dsSplit.setInput(input); dsSplit.setState(state); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp index 3798baa365..03d69b5f77 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp @@ -234,7 +234,6 @@ TEST_CASE("RobotDynamicsEstimator") kinDynWrapperList.emplace_back(std::make_shared()); REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); - REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState(true)); } // automatic build the Estimator from parameter handler @@ -247,6 +246,13 @@ TEST_CASE("RobotDynamicsEstimator") Eigen::VectorXd robotJointAcceleration(kinDyn->model().getNrOfDOFs()); robotJointAcceleration.setZero(); + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + REQUIRE(kinDynWrapperList.at(idx)->updateState(robotBaseAcceleration, + robotJointAcceleration, + true)); + } + iDynTree::LinkNetExternalWrenches extWrench(kinDyn->model()); extWrench.zero(); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp index 1d865c515a..c4f0bfdf85 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp @@ -219,7 +219,9 @@ TEST_CASE("SubModelKinDynWrapper") REQUIRE(kinDynSubModel.initialize(subModelList[idx])); - REQUIRE(kinDynSubModel.updateInternalKinDynState(true)); + REQUIRE(kinDynSubModel.updateState(robotBaseAcceleration, + robotJointAcceleration, + true)); int numberOfJoints = subModelList[idx].getModel().getNrOfDOFs(); @@ -236,9 +238,7 @@ TEST_CASE("SubModelKinDynWrapper") const std::string baseFrame = kinDynSubModel.getBaseFrameName(); manif::SE3d::Tangent baseAcceleration; - REQUIRE(kinDynSubModel.getBaseAcceleration(robotBaseAcceleration, - robotJointAcceleration, - baseAcceleration)); + baseAcceleration = kinDynSubModel.getBaseAcceleration(); manif::SE3d::Tangent baseAccelerationFromFullModel; REQUIRE(kinDyn->getFrameAcc(baseFrame, diff --git a/src/Estimators/tests/RobotDynamicsEstimator/UkfMeasurementTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/UkfMeasurementTest.cpp index acbec5c379..c6dcf5a71b 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/UkfMeasurementTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/UkfMeasurementTest.cpp @@ -108,7 +108,6 @@ TEST_CASE("UkfMeasurement") kinDynWrapperList.emplace_back(std::make_shared()); REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); - REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState(true)); } // Build the UkfState @@ -171,6 +170,13 @@ TEST_CASE("UkfMeasurement") input.robotBaseVelocity = baseVelocity; input.robotBaseAcceleration = baseAcceleration; + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + REQUIRE(kinDynWrapperList.at(idx)->updateState(baseAcceleration, + Eigen::VectorXd(kinDyn->model().getNrOfDOFs()).setZero(), + true)); + } + std::shared_ptr inputProvider = std::make_shared(); BipedalLocomotion::System::VariablesHandler measurementHandler = measurementModel->getMeasurementVariableHandler(); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/UkfStateTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/UkfStateTest.cpp index d12b8e703a..49de114f9f 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/UkfStateTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/UkfStateTest.cpp @@ -108,7 +108,6 @@ TEST_CASE("UkfState") kinDynWrapperList.emplace_back(std::make_shared()); REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); - REQUIRE(kinDynWrapperList.at(idx)->updateInternalKinDynState(true)); } // Build the UkfState @@ -149,6 +148,13 @@ TEST_CASE("UkfState") input.robotBaseVelocity = baseVelocity; input.robotBaseAcceleration = baseAcceleration; + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + REQUIRE(kinDynWrapperList.at(idx)->updateState(baseAcceleration, + Eigen::VectorXd(kinDyn->model().getNrOfDOFs()).setZero(), + true)); + } + std::shared_ptr inputProvider = std::make_shared(); BipedalLocomotion::System::VariablesHandler stateHandler = stateModel->getStateVariableHandler(); From e49bbd42e90d9fd51e14daf5a84bef2dd21116d1 Mon Sep 17 00:00:00 2001 From: Ines Date: Mon, 17 Apr 2023 15:34:35 +0200 Subject: [PATCH 08/13] Fix tests based on new robot model --- src/Estimators/src/SubModel.cpp | 4 --- src/Estimators/src/ZeroVelocityDynamics.cpp | 2 +- .../AccelerometerMeasurementDynamicsTest.cpp | 3 +- .../FrictionTorqueStateDynamicsTest.cpp | 3 +- .../GyroscopeMeasurementDynamicsTest.cpp | 3 +- .../JointVelocityStateDynamicsTest.cpp | 17 ++++++----- .../RobotDynamicsEstimatorTest.cpp | 29 +++++++++---------- .../SubModelCreatorTest.cpp | 3 +- .../SubModelKinDynWrapperTest.cpp | 2 +- .../UkfMeasurementTest.cpp | 28 +++++++++--------- .../RobotDynamicsEstimator/UkfStateTest.cpp | 8 ++--- .../RobotDynamicsEstimator/config/model.ini | 5 ++-- .../config/ukf/ukf_measurement.ini | 6 ++-- .../config/ukf/ukf_state.ini | 12 ++++---- 14 files changed, 62 insertions(+), 63 deletions(-) diff --git a/src/Estimators/src/SubModel.cpp b/src/Estimators/src/SubModel.cpp index fd7aa6e338..1985e91404 100644 --- a/src/Estimators/src/SubModel.cpp +++ b/src/Estimators/src/SubModel.cpp @@ -107,8 +107,6 @@ RDE::SubModelCreator::attachFTsToSubModel(iDynTree::Model& idynSubModel) ft.forceDirection = RDE::FT::Direction::Negative; } - log()->error("Creating Ft --> {}", ft.name); - ftList.push_back(ft); } else { @@ -156,8 +154,6 @@ RDE::SubModelCreator::attachFTsToSubModel(iDynTree::Model& idynSubModel) ft.forceDirection = RDE::FT::Direction::Negative; } - log()->error("Creating Ft --> {}", ft.name); - ftList.push_back(std::move(ft)); } } diff --git a/src/Estimators/src/ZeroVelocityDynamics.cpp b/src/Estimators/src/ZeroVelocityDynamics.cpp index 764ab8a830..34c674b302 100644 --- a/src/Estimators/src/ZeroVelocityDynamics.cpp +++ b/src/Estimators/src/ZeroVelocityDynamics.cpp @@ -122,7 +122,7 @@ bool RDE::ZeroVelocityDynamics::setSubModels(const std::vector& s bool RDE::ZeroVelocityDynamics::checkStateVariableHandler() { - constexpr auto errorPrefix = "[FrictionTorqueStateDynamics::checkStateVariableHandler]"; + constexpr auto errorPrefix = "[ZeroVelocityDynamics::checkStateVariableHandler]"; // Check if the variable handler contains the variables used by this dynamics if (!m_stateVariableHandler.getVariable(m_name).isValid()) diff --git a/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp index 59387799fe..24c4429b67 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp @@ -36,7 +36,7 @@ void createModelLoader(IParametersHandler::shared_ptr group, std::vector ftFramesList; auto ftGroup = group->getGroup("FT").lock(); - REQUIRE(ftGroup->getParameter("frames", ftFramesList)); + REQUIRE(ftGroup->getParameter("associated_joints", ftFramesList)); std::vector jointsAndFTs; jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); @@ -123,6 +123,7 @@ TEST_CASE("Friction Torque Dynamics") std::vector emptyVectorString; emptyGroupNamesFrames->setParameter("names", emptyVectorString); emptyGroupNamesFrames->setParameter("frames", emptyVectorString); + emptyGroupNamesFrames->setParameter("associated_joints", emptyVectorString); REQUIRE(modelParamHandler->setGroup("FT", emptyGroupNamesFrames)); REQUIRE(modelParamHandler->setGroup("GYROSCOPE", emptyGroupNamesFrames)); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp index 344324626f..29dd42d193 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp @@ -36,7 +36,7 @@ void createModelLoader(IParametersHandler::shared_ptr group, std::vector ftFramesList; auto ftGroup = group->getGroup("FT").lock(); - REQUIRE(ftGroup->getParameter("frames", ftFramesList)); + REQUIRE(ftGroup->getParameter("associated_joints", ftFramesList)); std::vector jointsAndFTs; jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); @@ -128,6 +128,7 @@ TEST_CASE("Friction Torque Dynamics") std::vector emptyVectorString; emptyGroupNamesFrames->setParameter("names", emptyVectorString); emptyGroupNamesFrames->setParameter("frames", emptyVectorString); + emptyGroupNamesFrames->setParameter("associated_joints", emptyVectorString); REQUIRE(modelParamHandler->setGroup("FT", emptyGroupNamesFrames)); REQUIRE(modelParamHandler->setGroup("ACCELEROMETER", emptyGroupNamesFrames)); REQUIRE(modelParamHandler->setGroup("GYROSCOPE", emptyGroupNamesFrames)); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp index f373c8a8b9..34d32f5533 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp @@ -36,7 +36,7 @@ void createModelLoader(IParametersHandler::shared_ptr group, std::vector ftFramesList; auto ftGroup = group->getGroup("FT").lock(); - REQUIRE(ftGroup->getParameter("frames", ftFramesList)); + REQUIRE(ftGroup->getParameter("associated_joints", ftFramesList)); std::vector jointsAndFTs; jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); @@ -123,6 +123,7 @@ TEST_CASE("Gyroscope Measurement Dynamics") std::vector emptyVectorString; emptyGroupNamesFrames->setParameter("names", emptyVectorString); emptyGroupNamesFrames->setParameter("frames", emptyVectorString); + emptyGroupNamesFrames->setParameter("associated_joints", emptyVectorString); REQUIRE(modelParamHandler->setGroup("FT", emptyGroupNamesFrames)); REQUIRE(modelParamHandler->setGroup("ACCELEROMETER", emptyGroupNamesFrames)); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp index 324a4ad2bc..565638d35d 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp @@ -76,7 +76,7 @@ void createModelLoader(IParametersHandler::shared_ptr group, { std::vector ftFramesList; auto ftGroup = group->getGroup("FT").lock(); - REQUIRE(ftGroup->getParameter("frames", ftFramesList)); + REQUIRE(ftGroup->getParameter("associated_joints", ftFramesList)); jointsAndFTs.insert(jointsAndFTs.end(), ftFramesList.begin(), ftFramesList.end()); } @@ -110,6 +110,7 @@ void createSubModels(iDynTree::ModelLoader& mdlLdr, std::vector emptyVector; emptySubGroup->setParameter("names", emptyVector); emptySubGroup->setParameter("frames", emptyVector); + emptySubGroup->setParameter("associated_joints", emptyVector); groupEmpty->setGroup("FT", emptySubGroup); groupEmpty->setGroup("ACCELEROMETER", emptySubGroup); groupEmpty->setGroup("GYROSCOPE", emptySubGroup); @@ -305,12 +306,12 @@ TEST_CASE("Joint Velocity Dynamics With FT") REQUIRE(variableHandler.addVariable("ds", sizeVariable)); REQUIRE(variableHandler.addVariable("tau_m", sizeVariable)); REQUIRE(variableHandler.addVariable("tau_F", sizeVariable)); - REQUIRE(variableHandler.addVariable("r_leg_ft_sensor", 6)); - REQUIRE(variableHandler.addVariable("r_foot_front_ft_sensor", 6)); - REQUIRE(variableHandler.addVariable("r_foot_rear_ft_sensor", 6)); - REQUIRE(variableHandler.addVariable("r_leg_ft_sensor_bias", 6)); - REQUIRE(variableHandler.addVariable("r_foot_front_ft_sensor_bias", 6)); - REQUIRE(variableHandler.addVariable("r_foot_rear_ft_sensor_bias", 6)); + REQUIRE(variableHandler.addVariable("r_leg_ft", 6)); + REQUIRE(variableHandler.addVariable("r_foot_front_ft", 6)); + REQUIRE(variableHandler.addVariable("r_foot_rear_ft", 6)); + REQUIRE(variableHandler.addVariable("r_leg_ft_bias", 6)); + REQUIRE(variableHandler.addVariable("r_foot_front_ft_bias", 6)); + REQUIRE(variableHandler.addVariable("r_foot_rear_ft_bias", 6)); REQUIRE(variableHandler.addVariable("r_leg_ft_acc_bias", 3)); REQUIRE(variableHandler.addVariable("r_foot_front_ft_acc_bias", 3)); REQUIRE(variableHandler.addVariable("r_foot_rear_ft_acc_bias", 3)); @@ -380,7 +381,7 @@ TEST_CASE("Joint Velocity Dynamics With FT") } auto massSecondSubModel = subModelListWithFT.at(1).getModel().getTotalMass(); - state(variableHandler.getVariable("r_leg_ft_sensor").offset+2) = massSecondSubModel * BipedalLocomotion::Math::StandardAccelerationOfGravitation; + state(variableHandler.getVariable("r_leg_ft").offset+2) = massSecondSubModel * BipedalLocomotion::Math::StandardAccelerationOfGravitation; // Create an input for the ukf state UKFInput input; diff --git a/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp index 03d69b5f77..a16948c0da 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp @@ -70,7 +70,7 @@ void createModelLoader(IParametersHandler::weak_ptr group, std::vector ftFramesList; auto ftGroup = group.lock()->getGroup("FT").lock(); - REQUIRE(ftGroup->getParameter("frames", ftFramesList)); + REQUIRE(ftGroup->getParameter("associated_joints", ftFramesList)); std::vector jointsAndFTs; jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); @@ -123,12 +123,12 @@ void createInitialState(std::shared_ptr kinDynFull initialState.ds.setZero(); initialState.tau_m << -1.24764e+01, 1.03400e-01, -4.70000e-03, -3.16350e+00, 4.21800e-01, 4.85000e-01; initialState.tau_F.setZero(); - initialState.ftWrenches["r_leg_ft_sensor"] << -3.83709731e+01, -8.45653659e+00, 9.25208925e+01, 3.12676978e+00, -9.79714657e+00, 4.01285264e-01; - initialState.ftWrenches["r_foot_front_ft_sensor"] << 5.22305136e-01, -4.88067107e-01, 1.61011089e+00, -9.13330381e-03, -8.39265034e-03, 4.18725767e-04; - initialState.ftWrenches["r_foot_rear_ft_sensor"] << 5.19688377e-01, -4.85621881e-01, 1.60204420e+00, -8.32204636e-03, -9.16952530e-03, -7.99299795e-05; - initialState.ftWrenchesBiases["r_leg_ft_sensor_bias"] << -1.89410386e+01, 7.33674253e+01, -6.04774355e+01, 1.34890092e-02, -3.02023625e+00, 7.01925185e-01; - initialState.ftWrenchesBiases["r_foot_front_ft_sensor_bias"] << -4.85874907e+01, -3.90627141e+01, -3.89636265e+01, -1.83127438e-01, 9.51385814e-01, 2.97127661e-01; - initialState.ftWrenchesBiases["r_foot_rear_ft_sensor_bias"] << -3.75985458e+01, -6.87282453e+01, -1.41893873e+00, -2.24845286e+00, 1.18104453e+00, 3.75446141e-01; + initialState.ftWrenches["r_leg_ft"] << -3.83709731e+01, -8.45653659e+00, 9.25208925e+01, 3.12676978e+00, -9.79714657e+00, 4.01285264e-01; + initialState.ftWrenches["r_foot_front_ft"] << 5.22305136e-01, -4.88067107e-01, 1.61011089e+00, -9.13330381e-03, -8.39265034e-03, 4.18725767e-04; + initialState.ftWrenches["r_foot_rear_ft"] << 5.19688377e-01, -4.85621881e-01, 1.60204420e+00, -8.32204636e-03, -9.16952530e-03, -7.99299795e-05; + initialState.ftWrenchesBiases["r_leg_ft_bias"] << -1.89410386e+01, 7.33674253e+01, -6.04774355e+01, 1.34890092e-02, -3.02023625e+00, 7.01925185e-01; + initialState.ftWrenchesBiases["r_foot_front_ft_bias"] << -4.85874907e+01, -3.90627141e+01, -3.89636265e+01, -1.83127438e-01, 9.51385814e-01, 2.97127661e-01; + initialState.ftWrenchesBiases["r_foot_rear_ft_bias"] << -3.75985458e+01, -6.87282453e+01, -1.41893873e+00, -2.24845286e+00, 1.18104453e+00, 3.75446141e-01; } void createInput(std::shared_ptr kinDynFullModel, @@ -168,17 +168,14 @@ void createInput(std::shared_ptr kinDynFullModel, input.motorCurrents.resize(kinDynFullModel->model().getNrOfDOFs()); input.motorCurrents << -1.124e+00, -2.200e-02, -1.000e-03, -2.850e-01, 3.800e-02, 1.940e-01; - input.ftWrenches["r_leg_ft_sensor"] = Eigen::VectorXd(6).setZero(); -// input.ftWrenches["r_leg_ft_sensor"](2) = subModelList.at(1).getModel().getTotalMass() * BipedalLocomotion::Math::StandardAccelerationOfGravitation; - input.ftWrenches["r_leg_ft_sensor"] << -57.31201172, 64.91088867, 32.04345703, 3.14025879, -12.81738281, 1.10321045; + input.ftWrenches["r_leg_ft"] = Eigen::VectorXd(6).setZero(); + input.ftWrenches["r_leg_ft"] << -57.31201172, 64.91088867, 32.04345703, 3.14025879, -12.81738281, 1.10321045; - input.ftWrenches["r_foot_front_ft_sensor"] = Eigen::VectorXd(6).setZero(); -// input.ftWrenches["r_foot_front_ft_sensor"](2) = subModelList.at(2).getModel().getTotalMass() * BipedalLocomotion::Math::StandardAccelerationOfGravitation; - input.ftWrenches["r_foot_front_ft_sensor"] << -48.06518555, -39.55078125, -37.35351562, -0.19226074, 0.94299316, 0.29754639; + input.ftWrenches["r_foot_front_ft"] = Eigen::VectorXd(6).setZero(); + input.ftWrenches["r_foot_front_ft"] << -48.06518555, -39.55078125, -37.35351562, -0.19226074, 0.94299316, 0.29754639; - input.ftWrenches["r_foot_rear_ft_sensor"] = Eigen::VectorXd(6).setZero(); -// input.ftWrenches["r_foot_rear_ft_sensor"](2) = subModelList.at(3).getModel().getTotalMass() * BipedalLocomotion::Math::StandardAccelerationOfGravitation; - input.ftWrenches["r_foot_rear_ft_sensor"] << -37.07885742, -69.21386719, 0.18310547, -2.2567749, 1.171875, 0.37536621; + input.ftWrenches["r_foot_rear_ft"] = Eigen::VectorXd(6).setZero(); + input.ftWrenches["r_foot_rear_ft"] << -37.07885742, -69.21386719, 0.18310547, -2.2567749, 1.171875, 0.37536621; std::vector accList; auto accGroup = modelHandler.lock()->getGroup("ACCELEROMETER").lock(); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/SubModelCreatorTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/SubModelCreatorTest.cpp index 1ef8503470..4c8fded0c4 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/SubModelCreatorTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/SubModelCreatorTest.cpp @@ -63,7 +63,7 @@ TEST_CASE("SubModel Creation") std::vector ftFramesList; auto ftGroup = groupModel->getGroup("FT").lock(); - REQUIRE(ftGroup->getParameter("frames", ftFramesList)); + REQUIRE(ftGroup->getParameter("associated_joints", ftFramesList)); std::vector jointsAndFTs; jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); @@ -92,6 +92,7 @@ TEST_CASE("SubModel Creation") std::vector emptyVector; groupFT->setParameter("names", emptyVector); groupFT->setParameter("frames", emptyVector); + groupFT->setParameter("associated_joints", emptyVector); groupModel->setGroup("FT", groupFT); REQUIRE(subModelCreatorWithoutFT.createSubModels(groupModel)); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp index c4f0bfdf85..b636edff19 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp @@ -156,7 +156,7 @@ TEST_CASE("SubModelKinDynWrapper") std::vector ftFramesList; auto ftGroup = group->getGroup("FT").lock(); - REQUIRE(ftGroup->getParameter("frames", ftFramesList)); + REQUIRE(ftGroup->getParameter("associated_joints", ftFramesList)); std::vector jointsAndFTs; jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/UkfMeasurementTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/UkfMeasurementTest.cpp index c6dcf5a71b..3d67d676c3 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/UkfMeasurementTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/UkfMeasurementTest.cpp @@ -68,7 +68,7 @@ void createModelLoader(IParametersHandler::weak_ptr group, std::vector ftFramesList; auto ftGroup = group.lock()->getGroup("FT").lock(); - REQUIRE(ftGroup->getParameter("frames", ftFramesList)); + REQUIRE(ftGroup->getParameter("associated_joints", ftFramesList)); std::vector jointsAndFTs; jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); @@ -128,12 +128,12 @@ TEST_CASE("UkfMeasurement") REQUIRE(stateVariableHandler.addVariable("ds", sizeVariable)); REQUIRE(stateVariableHandler.addVariable("tau_m", sizeVariable)); REQUIRE(stateVariableHandler.addVariable("tau_F", sizeVariable)); - REQUIRE(stateVariableHandler.addVariable("r_leg_ft_sensor", 6)); - REQUIRE(stateVariableHandler.addVariable("r_foot_front_ft_sensor", 6)); - REQUIRE(stateVariableHandler.addVariable("r_foot_rear_ft_sensor", 6)); - REQUIRE(stateVariableHandler.addVariable("r_leg_ft_sensor_bias", 6)); - REQUIRE(stateVariableHandler.addVariable("r_foot_front_ft_sensor_bias", 6)); - REQUIRE(stateVariableHandler.addVariable("r_foot_rear_ft_sensor_bias", 6)); + REQUIRE(stateVariableHandler.addVariable("r_leg_ft", 6)); + REQUIRE(stateVariableHandler.addVariable("r_foot_front_ft", 6)); + REQUIRE(stateVariableHandler.addVariable("r_foot_rear_ft", 6)); + REQUIRE(stateVariableHandler.addVariable("r_leg_ft_bias", 6)); + REQUIRE(stateVariableHandler.addVariable("r_foot_front_ft_bias", 6)); + REQUIRE(stateVariableHandler.addVariable("r_foot_rear_ft_bias", 6)); REQUIRE(stateVariableHandler.addVariable("r_leg_ft_acc_bias", 3)); REQUIRE(stateVariableHandler.addVariable("r_foot_front_ft_acc_bias", 3)); REQUIRE(stateVariableHandler.addVariable("r_foot_rear_ft_acc_bias", 3)); @@ -203,10 +203,10 @@ TEST_CASE("UkfMeasurement") wrenchFTFootRear << 0, 0, 1.752, 0.000876, 0.000649, 0; currentState.segment(stateVariableHandler.getVariable("tau_m").offset, stateVariableHandler.getVariable("tau_m").size) = motorTorques; - currentState.segment(stateVariableHandler.getVariable("r_leg_ft_sensor").offset, stateVariableHandler.getVariable("r_leg_ft_sensor").size) = wrenchFTtLeg; - currentState.segment(stateVariableHandler.getVariable("r_foot_front_ft_sensor").offset, stateVariableHandler.getVariable("r_foot_front_ft_sensor").size) = wrenchFTFootFront; - currentState.segment(stateVariableHandler.getVariable("r_foot_rear_ft_sensor").offset, stateVariableHandler.getVariable("r_foot_rear_ft_sensor").size) = wrenchFTFootRear; - currentState.segment(stateVariableHandler.getVariable("r_foot_rear_ft_sensor").offset, stateVariableHandler.getVariable("r_foot_rear_ft_sensor").size) = wrenchFTFootRear; + currentState.segment(stateVariableHandler.getVariable("r_leg_ft").offset, stateVariableHandler.getVariable("r_leg_ft").size) = wrenchFTtLeg; + currentState.segment(stateVariableHandler.getVariable("r_foot_front_ft").offset, stateVariableHandler.getVariable("r_foot_front_ft").size) = wrenchFTFootFront; + currentState.segment(stateVariableHandler.getVariable("r_foot_rear_ft").offset, stateVariableHandler.getVariable("r_foot_rear_ft").size) = wrenchFTFootRear; + currentState.segment(stateVariableHandler.getVariable("r_foot_rear_ft").offset, stateVariableHandler.getVariable("r_foot_rear_ft").size) = wrenchFTFootRear; REQUIRE(inputProvider->setInput(input)); @@ -216,9 +216,9 @@ TEST_CASE("UkfMeasurement") i_m.resize(kinDyn->model().getNrOfDOFs()); i_m << -0.1468, 0.2345, 0, -0.0667, 0.008, -0.000692; measurement["i_m"] = i_m; - measurement["r_leg_ft_sensor"] = wrenchFTtLeg; - measurement["r_foot_front_ft_sensor"] = wrenchFTFootFront; - measurement["r_foot_rear_ft_sensor"] = wrenchFTFootRear; + measurement["r_leg_ft"] = wrenchFTtLeg; + measurement["r_foot_front_ft"] = wrenchFTFootFront; + measurement["r_foot_rear_ft"] = wrenchFTFootRear; Eigen::Vector3d zeroVec3; zeroVec3.setZero(); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/UkfStateTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/UkfStateTest.cpp index 49de114f9f..b83b44bf7c 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/UkfStateTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/UkfStateTest.cpp @@ -68,7 +68,7 @@ void createModelLoader(IParametersHandler::weak_ptr group, std::vector ftFramesList; auto ftGroup = group.lock()->getGroup("FT").lock(); - REQUIRE(ftGroup->getParameter("frames", ftFramesList)); + REQUIRE(ftGroup->getParameter("associated_joints", ftFramesList)); std::vector jointsAndFTs; jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); @@ -180,9 +180,9 @@ TEST_CASE("UkfState") wrenchFTFootRear << 0, 0, 1.752, 0.000876, 0.000649, 0; currentState.segment(stateHandler.getVariable("tau_m").offset, stateHandler.getVariable("tau_m").size) = motorTorques; - currentState.segment(stateHandler.getVariable("r_leg_ft_sensor").offset, stateHandler.getVariable("r_leg_ft_sensor").size) = wrenchFTtLeg; - currentState.segment(stateHandler.getVariable("r_foot_front_ft_sensor").offset, stateHandler.getVariable("r_foot_front_ft_sensor").size) = wrenchFTFootFront; - currentState.segment(stateHandler.getVariable("r_foot_rear_ft_sensor").offset, stateHandler.getVariable("r_foot_rear_ft_sensor").size) = wrenchFTFootRear; + currentState.segment(stateHandler.getVariable("r_leg_ft").offset, stateHandler.getVariable("r_leg_ft").size) = wrenchFTtLeg; + currentState.segment(stateHandler.getVariable("r_foot_front_ft").offset, stateHandler.getVariable("r_foot_front_ft").size) = wrenchFTFootFront; + currentState.segment(stateHandler.getVariable("r_foot_rear_ft").offset, stateHandler.getVariable("r_foot_rear_ft").size) = wrenchFTFootRear; Eigen::VectorXd updatedState; updatedState.resize(stateSize); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/model.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/model.ini index ce16c383df..3b0061a9ea 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/config/model.ini +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/model.ini @@ -4,8 +4,9 @@ joint_list ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", frames ("") [FT] -names ("r_leg_ft_sensor", "r_foot_front_ft_sensor", "r_foot_rear_ft_sensor") -frames ("r_leg_ft_sensor", "r_foot_front_ft_sensor", "r_foot_rear_ft_sensor") +names ("r_leg_ft", "r_foot_front_ft", "r_foot_rear_ft") +frames ("r_leg_ft", "r_foot_front_ft", "r_foot_rear_ft") +associated_joints ("r_leg_ft_sensor", "r_foot_front_ft_sensor", "r_foot_rear_ft_sensor") [ACCELEROMETER] names ("r_leg_ft_acc", "r_foot_front_ft_acc", "r_foot_rear_ft_acc") diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini index 10ac7c8ae5..8653ef21f0 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini @@ -18,21 +18,21 @@ torque_constant (0.111, 0.047, 0.047, 0.111, 0.111, 0.025) dynamic_model "MotorCurrentMeasurementDynamics" [RIGHT_LEG_FT] -name "r_leg_ft_sensor" +name "r_leg_ft" elements ("fx", "fy", "fz", "mx", "my", "mz") covariance (1e-2, 1e-2, 1e-2, 1e-3, 1e-3, 1e-3) use_bias 1 dynamic_model "ZeroVelocityDynamics" [RIGHT_FOOT_FRONT_FT] -name "r_foot_front_ft_sensor" +name "r_foot_front_ft" elements ("fx", "fy", "fz", "mx", "my", "mz") covariance (1e-2, 1e-2, 1e-2, 1e-5, 1e-5, 1e-5) use_bias 1 dynamic_model "ZeroVelocityDynamics" [RIGHT_FOOT_REAR_FT] -name "r_foot_rear_ft_sensor" +name "r_foot_rear_ft" elements ("fx", "fy", "fz", "mx", "my", "mz") covariance (1e-2, 1e-2, 1e-2, 1e-5, 1e-5, 1e-5) use_bias 1 diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini index a39ef7c455..8ce1901184 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini @@ -28,42 +28,42 @@ friction_k2 (1.767, 5.64, 0.27, 2.0, 3.0, 0.0) initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) [RIGHT_LEG_FT] -name "r_leg_ft_sensor" +name "r_leg_ft" elements ("fx", "fy", "fz", "mx", "my", "mz") covariance (1e-3, 1e-3, 1e-3, 1e-4, 1e-4, 1e-4) initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) dynamic_model "ZeroVelocityDynamics" [RIGHT_FOOT_FRONT_FT] -name "r_foot_front_ft_sensor" +name "r_foot_front_ft" elements ("fx", "fy", "fz", "mx", "my", "mz") covariance (1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6) initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) dynamic_model "ZeroVelocityDynamics" [RIGHT_FOOT_REAR_FT] -name "r_foot_rear_ft_sensor" +name "r_foot_rear_ft" elements ("fx", "fy", "fz", "mx", "my", "mz") covariance (1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6) initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) dynamic_model "ZeroVelocityDynamics" [RIGHT_LEG_FT_BIAS] -name "r_leg_ft_sensor_bias" +name "r_leg_ft_bias" elements ("fx", "fy", "fz", "mx", "my", "mz") covariance (1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6) initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) dynamic_model "ZeroVelocityDynamics" [RIGHT_FOOT_FRONT_FT_BIAS] -name "r_foot_front_ft_sensor_bias" +name "r_foot_front_ft_bias" elements ("fx", "fy", "fz", "mx", "my", "mz") covariance (1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6) initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) dynamic_model "ZeroVelocityDynamics" [RIGHT_FOOT_REAR_FT_BIAS] -name "r_foot_rear_ft_sensor_bias" +name "r_foot_rear_ft_bias" elements ("fx", "fy", "fz", "mx", "my", "mz") covariance (1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6) initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) From 92389a3c7f175bb93eac8cb050f40d7459e2d502 Mon Sep 17 00:00:00 2001 From: Ines Date: Tue, 18 Apr 2023 13:50:47 +0200 Subject: [PATCH 09/13] Add check on state and measurement names to avoid seg fault. --- src/Estimators/src/RobotDynamicsEstimator.cpp | 85 +++++++++++++++++-- src/Estimators/src/UkfMeasurement.cpp | 11 ++- 2 files changed, 86 insertions(+), 10 deletions(-) diff --git a/src/Estimators/src/RobotDynamicsEstimator.cpp b/src/Estimators/src/RobotDynamicsEstimator.cpp index 74a92f8773..0dde710805 100644 --- a/src/Estimators/src/RobotDynamicsEstimator.cpp +++ b/src/Estimators/src/RobotDynamicsEstimator.cpp @@ -264,20 +264,37 @@ std::unique_ptr RobotDynamicsEstimator::build(std::weak_ bool RobotDynamicsEstimator::setInitialState(const Output& initialState) { + constexpr auto logPrefix = "[RobotDynamicsEstimator::setInitialState]"; + System::VariablesHandler::VariableDescription variable; if (m_pimpl->stateHandler.getVariable("ds", variable)) { + if (initialState.ds.size() != variable.size) + { + log()->error("{} Wrong size of variable `ds`. Found {}, expected {}.", logPrefix, initialState.ds.size(), variable.size); + return false; + } m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = initialState.ds; } if (m_pimpl->stateHandler.getVariable("tau_m", variable)) { + if (initialState.tau_m.size() != variable.size) + { + log()->error("{} Wrong size of variable `tau_m`. Found {}, expected {}.", logPrefix, initialState.tau_m.size(), variable.size); + return false; + } m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = initialState.tau_m; } if (m_pimpl->stateHandler.getVariable("tau_F", variable)) { + if (initialState.tau_F.size() != variable.size) + { + log()->error("{} Wrong size of variable `tau_F`. Found {}, expected {}.", logPrefix, initialState.tau_F.size(), variable.size); + return false; + } m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = initialState.tau_F; } @@ -285,6 +302,11 @@ bool RobotDynamicsEstimator::setInitialState(const Output& initialState) { if (m_pimpl->stateHandler.getVariable(key, variable)) { + if (val.size() != variable.size) + { + log()->error("{} Wrong size of variable `{}`. Found {}, expected {}.", logPrefix, val, val.size(), variable.size); + return false; + } m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = val; } } @@ -293,6 +315,11 @@ bool RobotDynamicsEstimator::setInitialState(const Output& initialState) { if (m_pimpl->stateHandler.getVariable(key, variable)) { + if (val.size() != variable.size) + { + log()->error("{} Wrong size of variable `{}`. Found {}, expected {}.", logPrefix, val, val.size(), variable.size); + return false; + } m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = val; } } @@ -301,6 +328,11 @@ bool RobotDynamicsEstimator::setInitialState(const Output& initialState) { if (m_pimpl->stateHandler.getVariable(key, variable)) { + if (val.size() != variable.size) + { + log()->error("{} Wrong size of variable `{}`. Found {}, expected {}.", logPrefix, val, val.size(), variable.size); + return false; + } m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = val; } } @@ -309,6 +341,11 @@ bool RobotDynamicsEstimator::setInitialState(const Output& initialState) { if (m_pimpl->stateHandler.getVariable(key, variable)) { + if (val.size() != variable.size) + { + log()->error("{} Wrong size of variable `{}`. Found {}, expected {}.", logPrefix, val, val.size(), variable.size); + return false; + } m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = val; } } @@ -406,7 +443,9 @@ bool RobotDynamicsEstimator::setInput(const Input & input) const Output& RobotDynamicsEstimator::getOutput() const { - if (m_pimpl->isInitialStateSet) + 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); @@ -419,26 +458,54 @@ const Output& RobotDynamicsEstimator::getOutput() const for (auto & [key, value] : m_pimpl->estimatorOutput.ftWrenches) { - m_pimpl->estimatorOutput.ftWrenches[key] = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable(key).offset, - m_pimpl->stateHandler.getVariable(key).size); + if (m_pimpl->stateHandler.getVariable(key).size > 0) + { + m_pimpl->estimatorOutput.ftWrenches[key] = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable(key).offset, + m_pimpl->stateHandler.getVariable(key).size); + } + else + { + log()->debug("{} Variable {} not found in the state vector.", logPrefix, key); + } } for (auto & [key, value] : m_pimpl->estimatorOutput.ftWrenchesBiases) { - m_pimpl->estimatorOutput.ftWrenchesBiases[key] = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable(key).offset, - m_pimpl->stateHandler.getVariable(key).size); + if (m_pimpl->stateHandler.getVariable(key).size > 0) + { + m_pimpl->estimatorOutput.ftWrenchesBiases[key] = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable(key).offset, + m_pimpl->stateHandler.getVariable(key).size); + } + else + { + log()->debug("{} Variable {} not found in the state vector.", logPrefix, key); + } } for (auto & [key, value] : m_pimpl->estimatorOutput.accelerometerBiases) { - m_pimpl->estimatorOutput.accelerometerBiases[key] = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable(key).offset, - m_pimpl->stateHandler.getVariable(key).size); + if (m_pimpl->stateHandler.getVariable(key).size > 0) + { + m_pimpl->estimatorOutput.accelerometerBiases[key] = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable(key).offset, + m_pimpl->stateHandler.getVariable(key).size); + } + else + { + log()->debug("{} Variable {} not found in the state vector.", logPrefix, key); + } } for (auto & [key, value] : m_pimpl->estimatorOutput.gyroscopeBiases) { - m_pimpl->estimatorOutput.gyroscopeBiases[key] = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable(key).offset, - m_pimpl->stateHandler.getVariable(key).size); + if (m_pimpl->stateHandler.getVariable(key).size > 0) + { + m_pimpl->estimatorOutput.gyroscopeBiases[key] = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable(key).offset, + m_pimpl->stateHandler.getVariable(key).size); + } + else + { + log()->debug("{} Variable {} not found in the state vector.", logPrefix, key); + } } } diff --git a/src/Estimators/src/UkfMeasurement.cpp b/src/Estimators/src/UkfMeasurement.cpp index 14ccbbf2bc..65ddd720d2 100644 --- a/src/Estimators/src/UkfMeasurement.cpp +++ b/src/Estimators/src/UkfMeasurement.cpp @@ -513,15 +513,24 @@ std::pair RDE::UkfMeasurement::measure(const bfl::Data& data) c bool RDE::UkfMeasurement::freeze(const bfl::Data& data) { + constexpr auto logPrefix = "[UkfMeasurement::freeze]"; + m_pimpl->measurementMap = bfl::any::any_cast>(data); for (auto& [name, dynamics] : m_pimpl->dynamicsList) { m_pimpl->offsetMeasurement = m_pimpl->measurementVariableHandler.getVariable(name).offset; + if(m_pimpl->measurementMap.count(name) == 0) + { + BipedalLocomotion::log()->error("{} Measurement with name `{}` not found.", logPrefix, name); + 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_pimpl->offsetMeasurement < (m_pimpl->measurementVariableHandler.getVariable(name).offset + m_pimpl->measurementVariableHandler.getVariable(name).size)) + while(m_pimpl->offsetMeasurement < + (m_pimpl->measurementVariableHandler.getVariable(name).offset + m_pimpl->measurementVariableHandler.getVariable(name).size)) { m_pimpl->measurement.segment(m_pimpl->offsetMeasurement, m_pimpl->measurementMap[name].size()) = m_pimpl->measurementMap[name]; From 30b66bf1ac9d17681df4afa79822e95733cdaeeb Mon Sep 17 00:00:00 2001 From: Ines Date: Wed, 12 Apr 2023 20:52:52 +0200 Subject: [PATCH 10/13] Implement python bindings for RobotDynamicsEstimator --- bindings/python/CMakeLists.txt | 1 + .../RobotDynamicsEstimator/CMakeLists.txt | 16 ++ .../bindings/RobotDynamicsEstimator/Module.h | 26 ++ .../RobotDynamicsEstimator.h | 46 ++++ .../RobotDynamicsEstimator/src/Module.cpp | 31 +++ .../src/RobotDynamicsEstimator.cpp | 233 ++++++++++++++++++ .../bipedal_locomotion_framework.cpp.in | 9 + .../RobotDynamicsEstimator.h | 16 +- src/Estimators/src/RobotDynamicsEstimator.cpp | 8 +- .../RobotDynamicsEstimatorTest.cpp | 12 +- 10 files changed, 380 insertions(+), 18 deletions(-) create mode 100644 bindings/python/RobotDynamicsEstimator/CMakeLists.txt create mode 100644 bindings/python/RobotDynamicsEstimator/include/BipedalLocomotion/bindings/RobotDynamicsEstimator/Module.h create mode 100644 bindings/python/RobotDynamicsEstimator/include/BipedalLocomotion/bindings/RobotDynamicsEstimator/RobotDynamicsEstimator.h create mode 100644 bindings/python/RobotDynamicsEstimator/src/Module.cpp create mode 100644 bindings/python/RobotDynamicsEstimator/src/RobotDynamicsEstimator.cpp diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index 4c80b9edf3..b998804ab7 100644 --- a/bindings/python/CMakeLists.txt +++ b/bindings/python/CMakeLists.txt @@ -16,6 +16,7 @@ add_subdirectory(TextLogging) add_subdirectory(Conversions) add_subdirectory(YarpUtilities) add_subdirectory(ContinuousDynamicalSystem) +add_subdirectory(RobotDynamicsEstimator) include(ConfigureFileWithCMakeIf) diff --git a/bindings/python/RobotDynamicsEstimator/CMakeLists.txt b/bindings/python/RobotDynamicsEstimator/CMakeLists.txt new file mode 100644 index 0000000000..9cef2a3969 --- /dev/null +++ b/bindings/python/RobotDynamicsEstimator/CMakeLists.txt @@ -0,0 +1,16 @@ +# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# BSD-3-Clause license. + +if(FRAMEWORK_COMPILE_RobotDynamicsEstimator) + + set(H_PREFIX include/BipedalLocomotion/bindings/RobotDynamicsEstimator) + + add_bipedal_locomotion_python_module( + NAME RobotDynamicsEstimatorBindings + SOURCES src/RobotDynamicsEstimator.cpp src/Module.cpp + HEADERS ${H_PREFIX}/RobotDynamicsEstimator.h ${H_PREFIX}/Module.h + LINK_LIBRARIES BipedalLocomotion::RobotDynamicsEstimator + ) + +endif() diff --git a/bindings/python/RobotDynamicsEstimator/include/BipedalLocomotion/bindings/RobotDynamicsEstimator/Module.h b/bindings/python/RobotDynamicsEstimator/include/BipedalLocomotion/bindings/RobotDynamicsEstimator/Module.h new file mode 100644 index 0000000000..6ab90185ec --- /dev/null +++ b/bindings/python/RobotDynamicsEstimator/include/BipedalLocomotion/bindings/RobotDynamicsEstimator/Module.h @@ -0,0 +1,26 @@ +/** + * @file Module.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_BINDINGS_ROBOT_DYNAMICS_ESTIMATOR_MODULE_H +#define BIPEDAL_LOCOMOTION_BINDINGS_ROBOT_DYNAMICS_ESTIMATOR_MODULE_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace RobotDynamicsEstimator +{ + +void CreateModule(pybind11::module& module); + +} // namespace RobotDynamicsEstimator +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_ROBOT_DYNAMICS_ESTIMATOR_MODULE_H diff --git a/bindings/python/RobotDynamicsEstimator/include/BipedalLocomotion/bindings/RobotDynamicsEstimator/RobotDynamicsEstimator.h b/bindings/python/RobotDynamicsEstimator/include/BipedalLocomotion/bindings/RobotDynamicsEstimator/RobotDynamicsEstimator.h new file mode 100644 index 0000000000..6ba0f96744 --- /dev/null +++ b/bindings/python/RobotDynamicsEstimator/include/BipedalLocomotion/bindings/RobotDynamicsEstimator/RobotDynamicsEstimator.h @@ -0,0 +1,46 @@ +/** + * @file RobotDynamicsEstimator.h + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_BINDINGS_ROBOT_DYNAMICS_ESTIMATOR_ROBOT_DYNAMICS_ESTIMATOR_H +#define BIPEDAL_LOCOMOTION_BINDINGS_ROBOT_DYNAMICS_ESTIMATOR_ROBOT_DYNAMICS_ESTIMATOR_H + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace RobotDynamicsEstimator +{ + +template bool setKinDyn(T& myclass, ::pybind11::object& obj) +{ + std::shared_ptr* cls + = pybind11::detail::swig_wrapped_pointer_to_pybind< + std::shared_ptr>(obj); + + if (cls == nullptr) + { + throw ::pybind11::value_error("Invalid input for the function. Please provide a valid object."); + } + + return myclass.setKinDyn(*cls); +}; + +void CreateRobotDynamicsEstimator(pybind11::module& module); +void CreateSubModel(pybind11::module& module); +void CreateSubModelCreator(pybind11::module& module); +void CreateSubModelKinDynWrapper(pybind11::module& module); + +} // namespace RobotDynamicsEstimator +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_ROBOT_DYNAMICS_ESTIMATOR_ROBOT_DYNAMICS_ESTIMATOR_H + diff --git a/bindings/python/RobotDynamicsEstimator/src/Module.cpp b/bindings/python/RobotDynamicsEstimator/src/Module.cpp new file mode 100644 index 0000000000..1e71855590 --- /dev/null +++ b/bindings/python/RobotDynamicsEstimator/src/Module.cpp @@ -0,0 +1,31 @@ +/** + * @file Module.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace RobotDynamicsEstimator +{ +void CreateModule(pybind11::module& module) +{ + module.doc() = "Robot Dynamics Estimator module."; + + CreateRobotDynamicsEstimator(module); + CreateSubModel(module); + CreateSubModelCreator(module); + CreateSubModelKinDynWrapper(module); + +} +} // namespace RobotDynamicsEstimator +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/RobotDynamicsEstimator/src/RobotDynamicsEstimator.cpp b/bindings/python/RobotDynamicsEstimator/src/RobotDynamicsEstimator.cpp new file mode 100644 index 0000000000..c01a37e446 --- /dev/null +++ b/bindings/python/RobotDynamicsEstimator/src/RobotDynamicsEstimator.cpp @@ -0,0 +1,233 @@ +/** + * @file RobotDynamicsEstimator.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace RobotDynamicsEstimator +{ + +void CreateRobotDynamicsEstimator(pybind11::module& module) +{ + namespace py = ::pybind11; + + using namespace BipedalLocomotion::System; + using namespace BipedalLocomotion::ParametersHandler; + using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; + + py::class_(module, "RobotDynamicsEstimatorInput") + .def(py::init()) + .def_readwrite("basePose", &RobotDynamicsEstimatorInput::basePose) + .def_readwrite("baseVelocity", &RobotDynamicsEstimatorInput::baseVelocity) + .def_readwrite("baseAcceleration", &RobotDynamicsEstimatorInput::baseAcceleration) + .def_readwrite("jointPositions", &RobotDynamicsEstimatorInput::jointPositions) + .def_readwrite("jointVelocities", &RobotDynamicsEstimatorInput::jointVelocities) + .def_readwrite("motorCurrents", &RobotDynamicsEstimatorInput::motorCurrents) + .def_readwrite("ftWrenches", &RobotDynamicsEstimatorInput::ftWrenches) + .def_readwrite("linearAccelerations", &RobotDynamicsEstimatorInput::linearAccelerations) + .def_readwrite("angularVelocities", &RobotDynamicsEstimatorInput::angularVelocities); + + py::class_(module, "RobotDynamicsEstimatorOutput") + .def(py::init()) + .def_readwrite("ds", &RobotDynamicsEstimatorOutput::ds) + .def_readwrite("tau_m", &RobotDynamicsEstimatorOutput::tau_m) + .def_readwrite("tau_F", &RobotDynamicsEstimatorOutput::tau_F) + .def_readwrite("ftWrenches", &RobotDynamicsEstimatorOutput::ftWrenches) + .def_readwrite("ftWrenchesBiases", &RobotDynamicsEstimatorOutput::ftWrenchesBiases) + .def_readwrite("accelerometerBiases", &RobotDynamicsEstimatorOutput::accelerometerBiases) + .def_readwrite("gyroscopeBiases", &RobotDynamicsEstimatorOutput::gyroscopeBiases) + .def(py::pickle([](const RobotDynamicsEstimatorOutput &output) { //__getstate__ + return py::make_tuple(output.ds, + output.tau_m, + output.tau_F, + output.ftWrenches, + output.ftWrenchesBiases, + output.accelerometerBiases, + output.gyroscopeBiases); + }, + [](py::tuple t) { // __setstate__ + if(t.size() != 7) + throw std::runtime_error("Invalid state!"); + RobotDynamicsEstimatorOutput rde; + rde.ds = t[0].cast(); + rde.tau_m = t[1].cast(); + 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>(); + + return rde; + } + )); + + BipedalLocomotion::bindings::System::CreateAdvanceable + (module, "RobotDynamicsEstimator"); + + py::class_> + (module, "RobotDynamicsEstimator") + .def(py::init()) + .def("finalize", + &BipedalLocomotion::Estimators::RobotDynamicsEstimator::RobotDynamicsEstimator::finalize, + py::arg("stateVariableHandler"), + py::arg("measurementVariableHandler"), + py::arg("kinDynFullModel")) + .def_static("build", + [](std::shared_ptr handler, + py::object& obj, + const std::vector& subModelList, + const std::vector>& kinDynWrapperList) -> + std::unique_ptr { + // get the kindyn computation object from the swig bindings + std::shared_ptr* cls + = py::detail::swig_wrapped_pointer_to_pybind< + std::shared_ptr>(obj); + if (cls == nullptr) + { + throw ::pybind11::value_error("Invalid input for the function. Please provide " + "an iDynTree::KinDynComputations object."); + } + + auto estimator = + BipedalLocomotion::Estimators::RobotDynamicsEstimator::RobotDynamicsEstimator::build(handler, + *cls, + subModelList, + kinDynWrapperList); + + return estimator; + }, + py::arg("handler"), + py::arg("kinDynFullModel"), + py::arg("subModelList"), + py::arg("kinDynWrapperList")) + .def("setInitialState", + [](BipedalLocomotion::Estimators::RobotDynamicsEstimator::RobotDynamicsEstimator& obj, + const BipedalLocomotion::Estimators::RobotDynamicsEstimator::RobotDynamicsEstimatorOutput& state) -> bool + { + return obj.setInitialState(state); + }, + py::arg("initialState")); +} + +void CreateSubModel(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; + using namespace BipedalLocomotion::System; + + py::class_(module, "SubModel") + .def(py::init()); +} + +void CreateSubModelCreator(pybind11::module& module) +{ + namespace py = ::pybind11; + + auto setModelAndSensors = [] (BipedalLocomotion::Estimators::RobotDynamicsEstimator::SubModelCreator& subModelCreator, + ::pybind11::object& model, + ::pybind11::object& sensors) + { + iDynTree::Model* modelPtr + = pybind11::detail::swig_wrapped_pointer_to_pybind< + iDynTree::Model>(model); + + if (modelPtr == nullptr) + { + throw ::pybind11::value_error("Invalid input for the function. Please provide an " + "iDynTree::Model object."); + } + + iDynTree::SensorsList* sensorsPtr + = pybind11::detail::swig_wrapped_pointer_to_pybind< + iDynTree::SensorsList>(sensors); + + if (sensorsPtr == nullptr) + { + throw ::pybind11::value_error("Invalid input for the function. Please provide an " + "iDynTree::SensorsList object."); + } + + subModelCreator.setModelAndSensors(*modelPtr, *sensorsPtr); + }; + + auto createSubModels = [] (BipedalLocomotion::Estimators::RobotDynamicsEstimator::SubModelCreator& subModelCreator, + std::shared_ptr& parameterHandler) + { + subModelCreator.createSubModels(parameterHandler); + }; + + py::class_(module, "SubModelCreator") + .def(py::init()) + .def("createSubModels", + createSubModels, + py::arg("parameterHandler")) + .def("setModelAndSensors", + setModelAndSensors, + py::arg("model"), + py::arg("sensors")) + .def("setKinDyn", + BipedalLocomotion::bindings::RobotDynamicsEstimator::setKinDyn< + BipedalLocomotion::Estimators::RobotDynamicsEstimator::SubModelCreator>, + py::arg("kinDyn")) + .def("getNrOfSubModels", + &BipedalLocomotion::Estimators::RobotDynamicsEstimator::SubModelCreator::getNrOfSubModels) + .def("getSubModelList", + &BipedalLocomotion::Estimators::RobotDynamicsEstimator::SubModelCreator::getSubModelList) + .def("getSubModel", + &BipedalLocomotion::Estimators::RobotDynamicsEstimator::SubModelCreator::getSubModel, + py::arg("index")); +} + +void CreateSubModelKinDynWrapper(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; + using namespace BipedalLocomotion::System; + + py::class_>(module, "SubModelKinDynWrapper") + .def(py::init()) + .def("setKinDyn", + BipedalLocomotion::bindings::RobotDynamicsEstimator::setKinDyn, + py::arg("kinDyn")) + .def("initialize", &SubModelKinDynWrapper::initialize) + .def("updateState", &SubModelKinDynWrapper::updateState) + .def("forwardDynamics", &SubModelKinDynWrapper::forwardDynamics) + .def("getBaseAcceleration", &SubModelKinDynWrapper::getBaseAcceleration) + .def("getBaseVelocity", &SubModelKinDynWrapper::getBaseVelocity) + .def("getBaseFrameName", &SubModelKinDynWrapper::getBaseFrameName) + .def("getMassMatrix", &SubModelKinDynWrapper::getMassMatrix) + .def("getGeneralizedForces", &SubModelKinDynWrapper::getGeneralizedForces) + .def("getFTJacobian", &SubModelKinDynWrapper::getFTJacobian) + .def("getAccelerometerJacobian", &SubModelKinDynWrapper::getAccelerometerJacobian) + .def("getGyroscopeJacobian", &SubModelKinDynWrapper::getGyroscopeJacobian) + .def("getExtContactJacobian", &SubModelKinDynWrapper::getExtContactJacobian) + .def("getAccelerometerBiasAcceleration", &SubModelKinDynWrapper::getAccelerometerBiasAcceleration) + .def("getAccelerometerRotation", &SubModelKinDynWrapper::getAccelerometerRotation); +} + +} // namespace RobotDynamicsEstimator +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/bipedal_locomotion_framework.cpp.in b/bindings/python/bipedal_locomotion_framework.cpp.in index e724644cfe..0e1c3b3436 100644 --- a/bindings/python/bipedal_locomotion_framework.cpp.in +++ b/bindings/python/bipedal_locomotion_framework.cpp.in @@ -82,6 +82,10 @@ #include @endcmakeif FRAMEWORK_COMPILE_ContinuousDynamicalSystem +@cmakeif FRAMEWORK_COMPILE_RobotDynamicsEstimator +#include +@endcmakeif FRAMEWORK_COMPILE_RobotDynamicsEstimator + // Create the Python module PYBIND11_MODULE(bindings, m) { @@ -182,4 +186,9 @@ PYBIND11_MODULE(bindings, m) py::module continuousDynamicalSystemModule = m.def_submodule("continuous_dynamical_system"); bindings::ContinuousDynamicalSystem::CreateModule(continuousDynamicalSystemModule); @endcmakeif FRAMEWORK_COMPILE_ContinuousDynamicalSystem + + @cmakeif FRAMEWORK_COMPILE_RobotDynamicsEstimator + py::module robotDynamicsEstimatorModule = m.def_submodule("robot_dynamics_estimator"); + bindings::RobotDynamicsEstimator::CreateModule(robotDynamicsEstimatorModule); + @endcmakeif FRAMEWORK_COMPILE_RobotDynamicsEstimator } diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/RobotDynamicsEstimator.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/RobotDynamicsEstimator.h index ffdc6fb200..abdb1464a0 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/RobotDynamicsEstimator.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/RobotDynamicsEstimator.h @@ -27,10 +27,10 @@ namespace RobotDynamicsEstimator { /** - * Input of the RobotDynamicsEstimator containing both the inputs and the + * RobotDynamicsEstimatorInput of the RobotDynamicsEstimator containing both the inputs and the * measurements for the Unscented Kalman Filter */ -struct Input +struct RobotDynamicsEstimatorInput { manif::SE3d basePose; /**< Pose describing the robot base position and orientation. */ manif::SE3d::Tangent baseVelocity; /**< Velocity of the robot base. */ @@ -44,10 +44,10 @@ struct Input }; /** - * Output of the RobotDynamicsEstimator which represents the estimation of the + * RobotDynamicsEstimatorOutput of the RobotDynamicsEstimator which represents the estimation of the * Unscented Kalman Filter */ -struct Output +struct RobotDynamicsEstimatorOutput { Eigen::VectorXd ds; /**< Joints velocities in rad per sec. */ Eigen::VectorXd tau_m; /**< Motor toruqes in Nm. */ @@ -67,7 +67,7 @@ struct Output * input using `setInput`, call the `advance` method to perform an estimation step, use `getOutput` * to get the estimation result. */ -class RobotDynamicsEstimator : public System::Advanceable +class RobotDynamicsEstimator : public System::Advanceable { /** * Private implementation @@ -135,7 +135,7 @@ class RobotDynamicsEstimator : public System::Advanceable * @param initialState a reference to an `Output` object. * @return true in case of success, false otherwise. */ - bool setInitialState(const Output& initialState); + bool setInitialState(const RobotDynamicsEstimatorOutput& initialState); /** * @brief Determines the validity of the object retrieved with getOutput() @@ -153,13 +153,13 @@ class RobotDynamicsEstimator : public System::Advanceable * Set the input for the estimator. * @param input is a struct containing the input of the estimator. */ - bool setInput(const Input& input) override; + bool setInput(const RobotDynamicsEstimatorInput& input) override; /** * Get the output of the ukf * @return A struct containing the ukf estimation result. */ - const Output& getOutput() const override; + const RobotDynamicsEstimatorOutput& getOutput() const override; }; // class RobotDynamicsEstimator diff --git a/src/Estimators/src/RobotDynamicsEstimator.cpp b/src/Estimators/src/RobotDynamicsEstimator.cpp index 0dde710805..da35ce6bfc 100644 --- a/src/Estimators/src/RobotDynamicsEstimator.cpp +++ b/src/Estimators/src/RobotDynamicsEstimator.cpp @@ -21,7 +21,7 @@ using namespace BipedalLocomotion; struct RobotDynamicsEstimator::Impl { - Output estimatorOutput; /**< Output of the estimator. */ + 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. */ @@ -262,7 +262,7 @@ std::unique_ptr RobotDynamicsEstimator::build(std::weak_ return estimator; } -bool RobotDynamicsEstimator::setInitialState(const Output& initialState) +bool RobotDynamicsEstimator::setInitialState(const RobotDynamicsEstimatorOutput& initialState) { constexpr auto logPrefix = "[RobotDynamicsEstimator::setInitialState]"; @@ -405,7 +405,7 @@ bool RobotDynamicsEstimator::advance() return true; } -bool RobotDynamicsEstimator::setInput(const Input & input) +bool RobotDynamicsEstimator::setInput(const RobotDynamicsEstimatorInput & input) { constexpr auto logPrefix = "[RobotDynamicsEstimator::setInput]"; @@ -441,7 +441,7 @@ bool RobotDynamicsEstimator::setInput(const Input & input) return true; } -const Output& RobotDynamicsEstimator::getOutput() const +const RobotDynamicsEstimatorOutput& RobotDynamicsEstimator::getOutput() const { constexpr auto logPrefix = "[RobotDynamicsEstimator::getOutput]"; diff --git a/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp index a16948c0da..f1a17b677b 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp @@ -22,7 +22,7 @@ #include #include -//LIBRARYTOTEST +// RDE #include #include #include @@ -83,7 +83,7 @@ void createInitialState(std::shared_ptr kinDynFull Eigen::Ref jointTorques, IParametersHandler::weak_ptr modelHandler, const std::vector& subModelList, - Output& initialState) + RobotDynamicsEstimatorOutput& initialState) { // All the values set here come from an experiment on the real robot @@ -135,7 +135,7 @@ void createInput(std::shared_ptr kinDynFullModel, Eigen::Ref jointTorques, IParametersHandler::weak_ptr modelHandler, const std::vector& subModelList, - Input& input) + RobotDynamicsEstimatorInput& input) { // All the values set here come from an experiment on the real robot @@ -262,7 +262,7 @@ TEST_CASE("RobotDynamicsEstimator") extWrench, jointTorques); - Output initialState; + RobotDynamicsEstimatorOutput initialState; createInitialState(kinDyn, iDynTree::toEigen(jointTorques.jointTorques()), modelHandler, @@ -271,7 +271,7 @@ TEST_CASE("RobotDynamicsEstimator") REQUIRE(estimator->setInitialState(initialState)); - Input measurement; + RobotDynamicsEstimatorInput measurement; createInput(kinDyn, iDynTree::toEigen(jointTorques.jointTorques()), modelHandler, @@ -285,5 +285,5 @@ TEST_CASE("RobotDynamicsEstimator") BipedalLocomotion::log()->error("{}", toc - tic); - Output result = estimator->getOutput(); + RobotDynamicsEstimatorOutput result = estimator->getOutput(); } From 20559de158db89f7b8cd2cddcac04911b876061d Mon Sep 17 00:00:00 2001 From: Ines Date: Wed, 26 Apr 2023 16:57:05 +0200 Subject: [PATCH 11/13] Fix bindings --- .../src/RobotDynamicsEstimator.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/bindings/python/RobotDynamicsEstimator/src/RobotDynamicsEstimator.cpp b/bindings/python/RobotDynamicsEstimator/src/RobotDynamicsEstimator.cpp index c01a37e446..e5d3393061 100644 --- a/bindings/python/RobotDynamicsEstimator/src/RobotDynamicsEstimator.cpp +++ b/bindings/python/RobotDynamicsEstimator/src/RobotDynamicsEstimator.cpp @@ -120,12 +120,12 @@ void CreateRobotDynamicsEstimator(pybind11::module& module) return estimator; }, py::arg("handler"), - py::arg("kinDynFullModel"), - py::arg("subModelList"), - py::arg("kinDynWrapperList")) - .def("setInitialState", - [](BipedalLocomotion::Estimators::RobotDynamicsEstimator::RobotDynamicsEstimator& obj, - const BipedalLocomotion::Estimators::RobotDynamicsEstimator::RobotDynamicsEstimatorOutput& state) -> bool + py::arg("kinDynFullModel"), + py::arg("subModelList"), + py::arg("kinDynWrapperList")) + .def("setInitialState", + [](BipedalLocomotion::Estimators::RobotDynamicsEstimator::RobotDynamicsEstimator& obj, + const BipedalLocomotion::Estimators::RobotDynamicsEstimator::RobotDynamicsEstimatorOutput& state) -> bool { return obj.setInitialState(state); }, From d183d30d097ba17c807dab26b538c7c5aa025df2 Mon Sep 17 00:00:00 2001 From: Ines Date: Wed, 26 Apr 2023 16:58:18 +0200 Subject: [PATCH 12/13] Fix bugs. --- .../AccelerometerMeasurementDynamics.h | 4 +- .../SubModelKinDynWrapper.h | 2 +- .../src/AccelerometerMeasurementDynamics.cpp | 30 +++- src/Estimators/src/SubModelKinDynWrapper.cpp | 8 +- src/Estimators/src/UkfMeasurement.cpp | 135 +++++++++++++----- src/Estimators/src/UkfState.cpp | 92 ++++++++---- src/Estimators/src/ZeroVelocityDynamics.cpp | 1 - .../AccelerometerMeasurementDynamicsTest.cpp | 2 +- .../RobotDynamicsEstimatorTest.cpp | 68 +++++---- .../RobotDynamicsEstimator/config/model.ini | 4 +- .../config/ukf/ukf_measurement.ini | 10 +- .../config/ukf/ukf_state.ini | 8 +- 12 files changed, 253 insertions(+), 111 deletions(-) diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h index 843a8e2358..e1afeeebf5 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h @@ -54,8 +54,8 @@ class AccelerometerMeasurementDynamics : public Dynamics Eigen::VectorXd m_Jsdotdot; Eigen::Vector3d m_accRg; Eigen::Vector3d m_vCrossW; - Eigen::Vector3d linVel; - Eigen::Vector3d angVel; + Eigen::Vector3d m_linVel; + Eigen::Vector3d m_angVel; public: /* diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h index 3d3f734165..723be4feec 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h @@ -85,7 +85,7 @@ class SubModelKinDynWrapper * updateDynamicsVariableState updates the value of all the member variables containing * information about the robot kinematics and dynamics */ - bool updateDynamicsVariableState(bool isCorrectStep); + bool updateDynamicsVariableState(bool updateRobotDynamicsOnly); /** * @brief Compute the contribution of external contacts on the joint torques. diff --git a/src/Estimators/src/AccelerometerMeasurementDynamics.cpp b/src/Estimators/src/AccelerometerMeasurementDynamics.cpp index 403f802515..c6bb2d7e82 100644 --- a/src/Estimators/src/AccelerometerMeasurementDynamics.cpp +++ b/src/Estimators/src/AccelerometerMeasurementDynamics.cpp @@ -150,6 +150,9 @@ bool RDE::AccelerometerMeasurementDynamics::finalize(const System::VariablesHand m_accFrameVel.setZero(); + m_linVel.setZero(); + m_angVel.setZero(); + return true; } @@ -248,17 +251,38 @@ bool RDE::AccelerometerMeasurementDynamics::update() // J_dot nu + base_J v_dot_base + joint_J s_dotdot - acc_Rot_world gravity + bias for (int index = 0; index < m_subModelsWithAccelerometer.size(); index++) { +// std::cout << "------------------------------------------------------- Submodel index " << m_subModelsWithAccelerometer[index] << ", acc name " << m_name << std::endl; + m_JdotNu = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerBiasAcceleration(m_name); +// std::cout << "m_JdotNu" << std::endl; +// std::cout << m_JdotNu << std::endl; + m_JvdotBase = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerJacobian(m_name).block(0, 0, 6, 6) * m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getBaseAcceleration().coeffs(); +// std::cout << "m_JvdotBase" << std::endl; +// std::cout << m_JvdotBase << std::endl; + m_accRg = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerRotation(m_name).rotation() * m_gravity; - linVel = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerVelocity(m_name).coeffs().segment(0, 3); - angVel = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerVelocity(m_name).coeffs().segment(3, 6); +// std::cout << "m_accRg" << std::endl; +// std::cout << m_accRg << std::endl; + +// std::cout << "accelerometer velocity\n" << m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerVelocity(m_name).coeffs() << std::endl; + + m_linVel = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerVelocity(m_name).coeffs().segment(0, 3); + m_angVel = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerVelocity(m_name).coeffs().segment(3, 3); + +// std::cout << "Lin vel" << std::endl; +// std::cout << m_linVel << std::endl; +// std::cout << "Ang vel" << std::endl; +// std::cout << m_angVel << std::endl; + + m_vCrossW = m_linVel.cross(m_angVel); - m_vCrossW = linVel.cross(angVel); +// std::cout << "m_vCrossW" << std::endl; +// std::cout << m_vCrossW << std::endl; m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) = m_JdotNu.segment(0, 3) + m_JvdotBase.segment(0, 3) - m_vCrossW - m_accRg + m_bias; diff --git a/src/Estimators/src/SubModelKinDynWrapper.cpp b/src/Estimators/src/SubModelKinDynWrapper.cpp index a0876de522..defc59c2d4 100644 --- a/src/Estimators/src/SubModelKinDynWrapper.cpp +++ b/src/Estimators/src/SubModelKinDynWrapper.cpp @@ -124,7 +124,7 @@ bool RDE::SubModelKinDynWrapper::initialize(const RDE::SubModel& subModel) bool RDE::SubModelKinDynWrapper::updateState(manif::SE3d::Tangent& robotBaseAcceleration, Eigen::Ref robotJointAcceleration, - bool isCorrectStep) + bool updateRobotDynamicsOnly) { constexpr auto logPrefix = "[BipedalLocomotion::Estimators::SubModelKinDynWrapper::" "updateKinDynState]"; @@ -169,10 +169,10 @@ bool RDE::SubModelKinDynWrapper::updateState(manif::SE3d::Tangent& robotBaseAcce return false; } - return updateDynamicsVariableState(isCorrectStep); + return updateDynamicsVariableState(updateRobotDynamicsOnly); } -bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState(bool isCorrectStep) +bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState(bool updateRobotDynamicsOnly) { constexpr auto logPrefix = "[BipedalLocomotion::Estimators::SubModelKinDynWrapper::" "updateDynamicsVariableState]"; @@ -206,7 +206,7 @@ bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState(bool isCorrectStep) } } - if (isCorrectStep) + if (!updateRobotDynamicsOnly) { // Update accelerometer jacobians, dJnu, rotMatrix for (int idx = 0; idx < m_subModel.getAccelerometerList().size(); idx++) diff --git a/src/Estimators/src/UkfMeasurement.cpp b/src/Estimators/src/UkfMeasurement.cpp index 65ddd720d2..9f4351d4de 100644 --- a/src/Estimators/src/UkfMeasurement.cpp +++ b/src/Estimators/src/UkfMeasurement.cpp @@ -32,10 +32,11 @@ struct RDE::UkfMeasurement::Impl Eigen::Vector3d gravity{0, 0, -Math::StandardAccelerationOfGravitation}; /**< Gravity vector. */ Eigen::MatrixXd covarianceR; /**< Covariance matrix. */ - std::size_t measurementSize; /**< Length of the measurement vector. */ + int measurementSize{0}; /**< Length of the measurement vector. */ double dT; /**< Sampling time */ - std::map> dynamicsList; /**< List of the dynamics composing the process model. */ + std::vector>> dynamicsList; +// std::map> dynamicsList; /**< List of the dynamics composing the process model. */ System::VariablesHandler measurementVariableHandler; /**< Variable handler describing the measurement vector. */ System::VariablesHandler stateVariableHandler; /**< Variable handler describing the state vector. */ @@ -74,6 +75,8 @@ struct RDE::UkfMeasurement::Impl int offsetMeasurement; /**< Offset used to fill the measurement vector. */ + bool updateRobotDynamicsOnly{false}; + void unpackState() { jointVelocityState = currentState.segment(stateVariableHandler.getVariable("ds").offset, @@ -124,13 +127,13 @@ struct RDE::UkfMeasurement::Impl // compute joint acceleration per each sub-model containing the accelerometer for (int subModelIdx = 0; subModelIdx < subModelList.size(); subModelIdx++) { + // Update the kindyn wrapper object of the submodel + kinDynWrapperList[subModelIdx]->updateState(ukfInput.robotBaseAcceleration, + jointAccelerationState, + updateRobotDynamicsOnly); + if (subModelList[subModelIdx].getModel().getNrOfDOFs() > 0) { - // Update the kindyn wrapper object of the submodel - kinDynWrapperList[subModelIdx]->updateState(ukfInput.robotBaseAcceleration, - jointAccelerationState, - false); - totalTorqueFromContacts[subModelIdx].setZero(); // Contribution of FT measurements @@ -222,26 +225,44 @@ bool RDE::UkfMeasurement::finalize(const System::VariablesHandler& handler) m_pimpl->measurementSize = 0; // finalize all the dynamics - for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// { +// if(!dynamics->finalize(handler)) +// { +// BipedalLocomotion::log()->error("{} Error while finalizing the dynamics named {}", logPrefix, name); +// return false; +// } + +// m_pimpl->measurementSize += dynamics->size(); +// } + for (int indexDyn1 = 0; indexDyn1 < m_pimpl->dynamicsList.size(); indexDyn1++) { - if(!dynamics->finalize(handler)) + if(!m_pimpl->dynamicsList[indexDyn1].second->finalize(handler)) { - BipedalLocomotion::log()->error("{} Error while finalizing the dynamics named {}", logPrefix, name); + BipedalLocomotion::log()->error("{} Error while finalizing the dynamics named {}", logPrefix, m_pimpl->dynamicsList[indexDyn1].first); return false; } - m_pimpl->measurementSize += dynamics->size(); + m_pimpl->measurementSize += m_pimpl->dynamicsList[indexDyn1].second->size(); } // Set value of measurement covariance matrix m_pimpl->covarianceR.resize(m_pimpl->measurementSize, m_pimpl->measurementSize); + m_pimpl->covarianceR.setZero(); + +// for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// { +// m_pimpl->measurementVariableHandler.addVariable(name, dynamics->getCovariance().size()); - for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// m_pimpl->covarianceR.block(m_pimpl->measurementVariableHandler.getVariable(name).offset, m_pimpl->measurementVariableHandler.getVariable(name).offset, +// m_pimpl->measurementVariableHandler.getVariable(name).size, m_pimpl->measurementVariableHandler.getVariable(name).size) = dynamics->getCovariance().asDiagonal(); +// } + for (int indexDyn2 = 0; indexDyn2 < m_pimpl->dynamicsList.size(); indexDyn2++) { - m_pimpl->measurementVariableHandler.addVariable(name, dynamics->getCovariance().size()); + m_pimpl->measurementVariableHandler.addVariable(m_pimpl->dynamicsList[indexDyn2].first, m_pimpl->dynamicsList[indexDyn2].second->getCovariance().size()); - m_pimpl->covarianceR.block(m_pimpl->measurementVariableHandler.getVariable(name).offset, m_pimpl->measurementVariableHandler.getVariable(name).offset, - m_pimpl->measurementVariableHandler.getVariable(name).size, m_pimpl->measurementVariableHandler.getVariable(name).size) = dynamics->getCovariance().asDiagonal(); + m_pimpl->covarianceR.block(m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).offset, m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).offset, + m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).size, m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).size) = m_pimpl->dynamicsList[indexDyn2].second->getCovariance().asDiagonal(); } m_pimpl->jointVelocityState.resize(m_pimpl->kinDynFullModel->model().getNrOfDOFs()); @@ -267,6 +288,8 @@ bool RDE::UkfMeasurement::finalize(const System::VariablesHandler& handler) m_pimpl->measurementDescription = bfl::VectorDescription(m_pimpl->measurementSize); + m_pimpl->predictedMeasurement.resize(m_pimpl->measurementSize, 2*m_pimpl->stateVariableHandler.getNumberOfVariables()+1); + m_pimpl->isFinalized = true; return true; @@ -375,7 +398,8 @@ std::unique_ptr RDE::UkfMeasurement::build(std::weak_ptrinitialize(dynamicsGroup); // add dynamics to the list - measurement->m_pimpl->dynamicsList.insert({dynamicsName, dynamicsInstance}); +// measurement->m_pimpl->dynamicsList.insert({dynamicsName, dynamicsInstance}); + measurement->m_pimpl->dynamicsList.emplace_back(dynamicsName, dynamicsInstance); } measurement->m_pimpl->inputDescription = bfl::VectorDescription(stateVariableHandler.getNumberOfVariables()); @@ -417,8 +441,6 @@ std::pair RDE::UkfMeasurement::predictedMeasure(const Eigen::Re { constexpr auto logPrefix = "[UkfMeasurement::propagate]"; - m_pimpl->predictedMeasurement.resize(m_pimpl->measurementSize, cur_states.cols()); - // Check that everything is initialized and set if (!m_pimpl->isFinalized) { @@ -435,9 +457,9 @@ std::pair RDE::UkfMeasurement::predictedMeasure(const Eigen::Re // Get input of ukf from provider m_pimpl->ukfInput = m_pimpl->ukfInputProvider->getOutput(); - for (int index = 0; index < cur_states.cols(); index++) + for (int sample = 0; sample < cur_states.cols(); sample++) { - m_pimpl->currentState = cur_states.block(0, index, cur_states.rows(), 1);; + m_pimpl->currentState = cur_states.block(0, sample, cur_states.rows(), 1);; m_pimpl->unpackState(); @@ -448,8 +470,6 @@ std::pair RDE::UkfMeasurement::predictedMeasure(const Eigen::Re m_pimpl->jointVelocityState, m_pimpl->gravity); - m_pimpl->unpackState(); - if (!m_pimpl->updateState()) { BipedalLocomotion::log()->error("{} The joint accelerations are not updated.", logPrefix); @@ -462,23 +482,39 @@ std::pair RDE::UkfMeasurement::predictedMeasure(const Eigen::Re // This could be parallelized // Update all the dynamics - for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// { +// std::cout << "variable name = " << name << " , variable offset = " << m_pimpl->measurementVariableHandler.getVariable(name).offset << std::endl; + +// dynamics->setState(m_pimpl->currentState); + +// dynamics->setInput(m_pimpl->ukfInput); + +// if (!dynamics->update()) +// { +// BipedalLocomotion::log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, name); +// throw std::runtime_error("Error"); +// } + +// m_pimpl->tempPredictedMeas.segment(m_pimpl->measurementVariableHandler.getVariable(name).offset, +// m_pimpl->measurementVariableHandler.getVariable(name).size) = dynamics->getUpdatedVariable(); +// } + for (int indexDyn = 0; indexDyn < m_pimpl->dynamicsList.size(); indexDyn++) { - dynamics->setState(m_pimpl->currentState); + m_pimpl->dynamicsList[indexDyn].second->setState(m_pimpl->currentState); - dynamics->setInput(m_pimpl->ukfInput); + m_pimpl->dynamicsList[indexDyn].second->setInput(m_pimpl->ukfInput); - if (!dynamics->update()) + if (!m_pimpl->dynamicsList[indexDyn].second->update()) { - BipedalLocomotion::log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, name); + BipedalLocomotion::log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, m_pimpl->dynamicsList[indexDyn].first); throw std::runtime_error("Error"); } - m_pimpl->tempPredictedMeas.segment(m_pimpl->measurementVariableHandler.getVariable(name).offset, - m_pimpl->measurementVariableHandler.getVariable(name).size) = dynamics->getUpdatedVariable(); + m_pimpl->tempPredictedMeas.segment(m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn].first).offset, + m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn].first).size) = m_pimpl->dynamicsList[indexDyn].second->getUpdatedVariable(); } - m_pimpl->predictedMeasurement.block(0, index, m_pimpl->measurementSize, 1) = m_pimpl->tempPredictedMeas; - + m_pimpl->predictedMeasurement.block(0, sample, m_pimpl->measurementSize, 1) = m_pimpl->tempPredictedMeas; } return std::make_pair(true, m_pimpl->predictedMeasurement); @@ -517,25 +553,46 @@ bool RDE::UkfMeasurement::freeze(const bfl::Data& data) m_pimpl->measurementMap = bfl::any::any_cast>(data); - for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// { +// m_pimpl->offsetMeasurement = m_pimpl->measurementVariableHandler.getVariable(name).offset; + +// if(m_pimpl->measurementMap.count(name) == 0) +// { +// BipedalLocomotion::log()->error("{} Measurement with name `{}` not found.", logPrefix, name); +// 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_pimpl->offsetMeasurement < +// (m_pimpl->measurementVariableHandler.getVariable(name).offset + m_pimpl->measurementVariableHandler.getVariable(name).size)) +// { +// m_pimpl->measurement.segment(m_pimpl->offsetMeasurement, m_pimpl->measurementMap[name].size()) +// = m_pimpl->measurementMap[name]; + +// m_pimpl->offsetMeasurement += m_pimpl->measurementMap[name].size(); +// } +// } + for (int index = 0; index < m_pimpl->dynamicsList.size(); index++) { - m_pimpl->offsetMeasurement = m_pimpl->measurementVariableHandler.getVariable(name).offset; + m_pimpl->offsetMeasurement = m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[index].first).offset; - if(m_pimpl->measurementMap.count(name) == 0) + if(m_pimpl->measurementMap.count(m_pimpl->dynamicsList[index].first) == 0) { - BipedalLocomotion::log()->error("{} Measurement with name `{}` not found.", logPrefix, name); + BipedalLocomotion::log()->error("{} Measurement with name `{}` not found.", logPrefix, m_pimpl->dynamicsList[index].first); return false; } // If more sub-models share the same accelerometer or gyroscope sensor, the measurement vector is concatenated // a number of times equal to the number of sub-models using the sensor. while(m_pimpl->offsetMeasurement < - (m_pimpl->measurementVariableHandler.getVariable(name).offset + m_pimpl->measurementVariableHandler.getVariable(name).size)) + (m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[index].first).offset + m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[index].first).size)) { - m_pimpl->measurement.segment(m_pimpl->offsetMeasurement, m_pimpl->measurementMap[name].size()) - = m_pimpl->measurementMap[name]; + m_pimpl->measurement.segment(m_pimpl->offsetMeasurement, m_pimpl->measurementMap[m_pimpl->dynamicsList[index].first].size()) + = m_pimpl->measurementMap[m_pimpl->dynamicsList[index].first]; - m_pimpl->offsetMeasurement += m_pimpl->measurementMap[name].size(); + m_pimpl->offsetMeasurement += m_pimpl->measurementMap[m_pimpl->dynamicsList[index].first].size(); } } diff --git a/src/Estimators/src/UkfState.cpp b/src/Estimators/src/UkfState.cpp index d2a08e1ee3..b0f0ca6991 100644 --- a/src/Estimators/src/UkfState.cpp +++ b/src/Estimators/src/UkfState.cpp @@ -34,7 +34,7 @@ struct RDE::UkfState::Impl std::size_t stateSize; /**< Length of the state vector. */ double dT; /**< Sampling time */ - std::map> dynamicsList; /**< List of the dynamics composing the process model. */ + std::vector>> dynamicsList; /**< List of the dynamics composing the process model. */ System::VariablesHandler stateVariableHandler; /**< Variable handler describing the state vector. */ @@ -63,6 +63,8 @@ struct RDE::UkfState::Impl std::vector torqueFromContact; /**< Joint torques due to a specific contact. */ Math::Wrenchd wrench; /**< Joint torques due to a specific contact. */ + bool updateRobotDynamicsOnly{true}; + void unpackState() { jointVelocityState = currentState.segment(stateVariableHandler.getVariable("ds").offset, @@ -110,16 +112,16 @@ struct RDE::UkfState::Impl jointVelocityState, gravity); - // compute joint acceleration per each sub-model containing the accelerometer + // compute joint acceleration per each sub-model for (int subModelIdx = 0; subModelIdx < subModelList.size(); subModelIdx++) { + // Update the kindyn wrapper object of the submodel + kinDynWrapperList[subModelIdx]->updateState(ukfInput.robotBaseAcceleration, + jointAccelerationState, + updateRobotDynamicsOnly); + if (subModelList[subModelIdx].getModel().getNrOfDOFs() > 0) { - // Update the kindyn wrapper object of the submodel - kinDynWrapperList[subModelIdx]->updateState(ukfInput.robotBaseAcceleration, - jointAccelerationState, - false); - totalTorqueFromContacts[subModelIdx].setZero(); // Contribution of FT measurements @@ -211,15 +213,25 @@ bool RDE::UkfState::finalize(const System::VariablesHandler& handler) m_pimpl->stateSize = 0; // finalize all the dynamics - for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// { +// if(!dynamics->finalize(handler)) +// { +// log()->error("{} Error while finalizing the dynamics named {}", logPrefix, name); +// return false; +// } + +// m_pimpl->stateSize += dynamics->size(); +// } + for (int indexDyn1 = 0; indexDyn1 < m_pimpl->dynamicsList.size(); indexDyn1++) { - if(!dynamics->finalize(handler)) + if(!m_pimpl->dynamicsList[indexDyn1].second->finalize(handler)) { - log()->error("{} Error while finalizing the dynamics named {}", logPrefix, name); + log()->error("{} Error while finalizing the dynamics named {}", logPrefix, m_pimpl->dynamicsList[indexDyn1].first); return false; } - m_pimpl->stateSize += dynamics->size(); + m_pimpl->stateSize += m_pimpl->dynamicsList[indexDyn1].second->size(); } @@ -230,13 +242,22 @@ bool RDE::UkfState::finalize(const System::VariablesHandler& handler) m_pimpl->initialCovariance.resize(m_pimpl->stateSize, m_pimpl->stateSize); m_pimpl->initialCovariance.setZero(); - for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// { +// m_pimpl->covarianceQ.block(handler.getVariable(name).offset, handler.getVariable(name).offset, +// handler.getVariable(name).size, handler.getVariable(name).size) = dynamics->getCovariance().asDiagonal(); + +// m_pimpl->initialCovariance.block(handler.getVariable(name).offset, handler.getVariable(name).offset, +// handler.getVariable(name).size, handler.getVariable(name).size) = dynamics->getInitialStateCovariance().asDiagonal(); + +// } + for (int indexDyn2 = 0; indexDyn2 < m_pimpl->dynamicsList.size(); indexDyn2++) { - m_pimpl->covarianceQ.block(handler.getVariable(name).offset, handler.getVariable(name).offset, - handler.getVariable(name).size, handler.getVariable(name).size) = dynamics->getCovariance().asDiagonal(); + m_pimpl->covarianceQ.block(handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).offset, handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).offset, + handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).size, handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).size) = m_pimpl->dynamicsList[indexDyn2].second->getCovariance().asDiagonal(); - m_pimpl->initialCovariance.block(handler.getVariable(name).offset, handler.getVariable(name).offset, - handler.getVariable(name).size, handler.getVariable(name).size) = dynamics->getInitialStateCovariance().asDiagonal(); + m_pimpl->initialCovariance.block(handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).offset, handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).offset, + handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).size, handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).size) = m_pimpl->dynamicsList[indexDyn2].second->getInitialStateCovariance().asDiagonal(); } @@ -370,7 +391,8 @@ std::unique_ptr RDE::UkfState::build(std::weak_ptrinitialize(dynamicsGroup); // add dynamics to the list - state->m_pimpl->dynamicsList.insert({dynamicsName, dynamicsInstance}); + state->m_pimpl->dynamicsList.emplace_back(dynamicsName, dynamicsInstance); +// state->m_pimpl->dynamicsList.insert({dynamicsName, dynamicsInstance}); } // finalize estimator @@ -423,9 +445,9 @@ void RDE::UkfState::propagate(const Eigen::Ref& cur_state prop_states.resize(cur_states.rows(), cur_states.cols()); - for (int index = 0; index < cur_states.cols(); index++) + for (int sample = 0; sample < cur_states.cols(); sample++) { - m_pimpl->currentState = cur_states.block(0, index, cur_states.rows(), 1); + m_pimpl->currentState = cur_states.block(0, sample, cur_states.rows(), 1); m_pimpl->unpackState(); @@ -441,23 +463,39 @@ void RDE::UkfState::propagate(const Eigen::Ref& cur_state // This could be parallelized // Update all the dynamics - for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// { +// dynamics->setState(m_pimpl->currentState); + +// dynamics->setInput(m_pimpl->ukfInput); + +// if (!dynamics->update()) +// { +// log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, name); +// throw std::runtime_error("Error"); +// } + +// m_pimpl->nextState.segment(m_pimpl->stateVariableHandler.getVariable(name).offset, +// m_pimpl->stateVariableHandler.getVariable(name).size) = dynamics->getUpdatedVariable(); +// } + + for (int indexDyn = 0; indexDyn < m_pimpl->dynamicsList.size(); indexDyn++) { - dynamics->setState(m_pimpl->currentState); + m_pimpl->dynamicsList[indexDyn].second->setState(m_pimpl->currentState); - dynamics->setInput(m_pimpl->ukfInput); + m_pimpl->dynamicsList[indexDyn].second->setInput(m_pimpl->ukfInput); - if (!dynamics->update()) + if (!m_pimpl->dynamicsList[indexDyn].second->update()) { - log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, name); + log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, m_pimpl->dynamicsList[indexDyn].first); throw std::runtime_error("Error"); } - m_pimpl->nextState.segment(m_pimpl->stateVariableHandler.getVariable(name).offset, - m_pimpl->stateVariableHandler.getVariable(name).size) = dynamics->getUpdatedVariable(); + m_pimpl->nextState.segment(m_pimpl->stateVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn].first).offset, + m_pimpl->stateVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn].first).size) = m_pimpl->dynamicsList[indexDyn].second->getUpdatedVariable(); } - prop_states.block(0, index, cur_states.rows(), 1) = m_pimpl->nextState; + prop_states.block(0, sample, cur_states.rows(), 1) = m_pimpl->nextState; } } diff --git a/src/Estimators/src/ZeroVelocityDynamics.cpp b/src/Estimators/src/ZeroVelocityDynamics.cpp index 34c674b302..ca87553cdd 100644 --- a/src/Estimators/src/ZeroVelocityDynamics.cpp +++ b/src/Estimators/src/ZeroVelocityDynamics.cpp @@ -62,7 +62,6 @@ bool RDE::ZeroVelocityDynamics::initialize(std::weak_ptr(); std::vector accNameList = {"r_leg_ft_acc"}; - std::vector accFrameList = {"r_leg_ft_sensor"}; + std::vector accFrameList = {"r_leg_ft"}; accGroup->setParameter("names", accNameList); accGroup->setParameter("frames", accFrameList); REQUIRE(modelParamHandler->setGroup("ACCELEROMETER", accGroup)); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp index f1a17b677b..beb57a0fa9 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp @@ -121,14 +121,19 @@ void createInitialState(std::shared_ptr kinDynFull // Set values initialState.ds.setZero(); - initialState.tau_m << -1.24764e+01, 1.03400e-01, -4.70000e-03, -3.16350e+00, 4.21800e-01, 4.85000e-01; +// initialState.tau_m << -1.24764e+01, 1.03400e-01, -4.70000e-03, -3.16350e+00, 4.21800e-01, 4.85000e-01; + initialState.tau_m << 22.644 , 13.5454, -7.6093, 19.3029, -15.0072, -0.805; initialState.tau_F.setZero(); - initialState.ftWrenches["r_leg_ft"] << -3.83709731e+01, -8.45653659e+00, 9.25208925e+01, 3.12676978e+00, -9.79714657e+00, 4.01285264e-01; - initialState.ftWrenches["r_foot_front_ft"] << 5.22305136e-01, -4.88067107e-01, 1.61011089e+00, -9.13330381e-03, -8.39265034e-03, 4.18725767e-04; - initialState.ftWrenches["r_foot_rear_ft"] << 5.19688377e-01, -4.85621881e-01, 1.60204420e+00, -8.32204636e-03, -9.16952530e-03, -7.99299795e-05; - initialState.ftWrenchesBiases["r_leg_ft_bias"] << -1.89410386e+01, 7.33674253e+01, -6.04774355e+01, 1.34890092e-02, -3.02023625e+00, 7.01925185e-01; - initialState.ftWrenchesBiases["r_foot_front_ft_bias"] << -4.85874907e+01, -3.90627141e+01, -3.89636265e+01, -1.83127438e-01, 9.51385814e-01, 2.97127661e-01; - initialState.ftWrenchesBiases["r_foot_rear_ft_bias"] << -3.75985458e+01, -6.87282453e+01, -1.41893873e+00, -2.24845286e+00, 1.18104453e+00, 3.75446141e-01; +// initialState.ftWrenches["r_leg_ft"] << -3.83709731e+01, -8.45653659e+00, 9.25208925e+01, 3.12676978e+00, -9.79714657e+00, 4.01285264e-01; +// initialState.ftWrenches["r_foot_front_ft"] << 5.22305136e-01, -4.88067107e-01, 1.61011089e+00, -9.13330381e-03, -8.39265034e-03, 4.18725767e-04; +// initialState.ftWrenches["r_foot_rear_ft"] << 5.19688377e-01, -4.85621881e-01, 1.60204420e+00, -8.32204636e-03, -9.16952530e-03, -7.99299795e-05; +// initialState.ftWrenchesBiases["r_leg_ft_bias"] << -1.89410386e+01, 7.33674253e+01, -6.04774355e+01, 1.34890092e-02, -3.02023625e+00, 7.01925185e-01; +// initialState.ftWrenchesBiases["r_foot_front_ft_bias"] << -4.85874907e+01, -3.90627141e+01, -3.89636265e+01, -1.83127438e-01, 9.51385814e-01, 2.97127661e-01; +// initialState.ftWrenchesBiases["r_foot_rear_ft_bias"] << -3.75985458e+01, -6.87282453e+01, -1.41893873e+00, -2.24845286e+00, 1.18104453e+00, 3.75446141e-01; + + initialState.ftWrenches["r_leg_ft"] << 49.45480758, 73.91349929, 46.85057916, -15.38562865, 9.04317083, 1.97395434; + initialState.ftWrenches["r_foot_front_ft"] << 1.38697938e-01, 8.71035288e-01, 1.52496873e+00, 1.36409975e-02, -1.99708849e-03, -9.99651158e-05; + initialState.ftWrenches["r_foot_rear_ft"] << 1.38003059e-01, 8.66671384e-01, 1.51732861e+00, 1.70397864e-02, -2.03056058e-03, -3.89970831e-04; } void createInput(std::shared_ptr kinDynFullModel, @@ -152,7 +157,8 @@ void createInput(std::shared_ptr kinDynFullModel, input.baseAcceleration = baseAcc; input.jointPositions.resize(kinDynFullModel->model().getNrOfDOFs()); - input.jointPositions << -3.81866275e-01, 1.27512464e-01, 3.83496133e-04, -2.67488553e-02, -9.77915140e-03, 9.58740333e-05; +// input.jointPositions << -3.81866275e-01, 1.27512464e-01, 3.83496133e-04, -2.67488553e-02, -9.77915140e-03, 9.58740333e-05; + input.jointPositions << 0.91693925, 0.69777121, 0.00249272, -0.68166438, -0.07698685, -0.08628663; input.jointVelocities.resize(kinDynFullModel->model().getNrOfDOFs()); input.jointVelocities.setZero(); @@ -166,16 +172,20 @@ void createInput(std::shared_ptr kinDynFullModel, // input.motorCurrents = jointTorques.array() / (gearRatio.array() * torqueConstant.array()); input.motorCurrents.resize(kinDynFullModel->model().getNrOfDOFs()); - input.motorCurrents << -1.124e+00, -2.200e-02, -1.000e-03, -2.850e-01, 3.800e-02, 1.940e-01; +// input.motorCurrents << -1.124e+00, -2.200e-02, -1.000e-03, -2.850e-01, 3.800e-02, 1.940e-01; + input.motorCurrents << 2.04 , -2.882, -1.619, 1.739, -1.352, -0.322; input.ftWrenches["r_leg_ft"] = Eigen::VectorXd(6).setZero(); - input.ftWrenches["r_leg_ft"] << -57.31201172, 64.91088867, 32.04345703, 3.14025879, -12.81738281, 1.10321045; +// input.ftWrenches["r_leg_ft"] << -57.31201172, 64.91088867, 32.04345703, 3.14025879, -12.81738281, 1.10321045; + input.ftWrenches["r_leg_ft"] << 49.45480758, 73.91349929, 46.85057916, -15.38562865, 9.04317083, 1.97395434; input.ftWrenches["r_foot_front_ft"] = Eigen::VectorXd(6).setZero(); - input.ftWrenches["r_foot_front_ft"] << -48.06518555, -39.55078125, -37.35351562, -0.19226074, 0.94299316, 0.29754639; +// input.ftWrenches["r_foot_front_ft"] << -48.06518555, -39.55078125, -37.35351562, -0.19226074, 0.94299316, 0.29754639; + input.ftWrenches["r_foot_front_ft"] << 1.38697938e-01, 8.71035288e-01, 1.52496873e+00, 1.36409975e-02, -1.99708849e-03, -9.99651158e-05; input.ftWrenches["r_foot_rear_ft"] = Eigen::VectorXd(6).setZero(); - input.ftWrenches["r_foot_rear_ft"] << -37.07885742, -69.21386719, 0.18310547, -2.2567749, 1.171875, 0.37536621; +// input.ftWrenches["r_foot_rear_ft"] << -37.07885742, -69.21386719, 0.18310547, -2.2567749, 1.171875, 0.37536621; + input.ftWrenches["r_foot_rear_ft"] << 1.38003059e-01, 8.66671384e-01, 1.51732861e+00, 1.70397864e-02, -2.03056058e-03, -3.89970831e-04; std::vector accList; auto accGroup = modelHandler.lock()->getGroup("ACCELEROMETER").lock(); @@ -185,9 +195,12 @@ void createInput(std::shared_ptr kinDynFullModel, input.linearAccelerations[acc] = Eigen::VectorXd(3).setZero(); input.linearAccelerations[acc](2) = - BipedalLocomotion::Math::StandardAccelerationOfGravitation; } - input.linearAccelerations["r_leg_ft_acc"] << 3.72, 0.8, -9.33; - input.linearAccelerations["r_foot_front_ft_acc"] << 4.01, -2.38, 8.74; - input.linearAccelerations["r_foot_rear_ft_acc"] << 3.77, -2.39, 8.94; +// input.linearAccelerations["r_leg_ft_acc"] << 3.72, 0.8, -9.33; +// input.linearAccelerations["r_foot_front_ft_acc"] << 4.01, -2.38, 8.74; +// input.linearAccelerations["r_foot_rear_ft_acc"] << 3.77, -2.39, 8.94; + input.linearAccelerations["r_leg_ft_acc"] << -3.76, -7.65, -4.32; + input.linearAccelerations["r_foot_front_ft_acc"] << 1.44, 5.08, 8.11; + input.linearAccelerations["r_foot_rear_ft_acc"] << 2.12, 5.23, 8.13; std::vector gyroList; auto gyroGroup = modelHandler.lock()->getGroup("GYROSCOPE").lock(); @@ -196,9 +209,12 @@ void createInput(std::shared_ptr kinDynFullModel, { input.angularVelocities[gyro] = Eigen::VectorXd(3).setZero(); } - input.angularVelocities["r_leg_ft_gyro"] << 0.03708825, -0.06654068, -0.00109083; - input.angularVelocities["r_foot_front_ft_gyro"] << 0.0567232 , 0.04799655, -0.01308997; - input.angularVelocities["r_foot_rear_ft_gyro"] << 0.06108652, 0.04690572, -0.01090831; +// input.angularVelocities["r_leg_ft_gyro"] << 0.03708825, -0.06654068, -0.00109083; +// input.angularVelocities["r_foot_front_ft_gyro"] << 0.0567232 , 0.04799655, -0.01308997; +// input.angularVelocities["r_foot_rear_ft_gyro"] << 0.06108652, 0.04690572, -0.01090831; + input.angularVelocities["r_leg_ft_gyro"] << -0.00654498, 0.0, 0.00109083; + input.angularVelocities["r_foot_front_ft_gyro"] << -0.00327249, 0.0, 0.00218166; + input.angularVelocities["r_foot_rear_ft_gyro"] << -0.00545415, 0.00109083, 0.00218166; } TEST_CASE("RobotDynamicsEstimator") @@ -278,12 +294,16 @@ TEST_CASE("RobotDynamicsEstimator") subModelList, measurement); - auto tic = BipedalLocomotion::clock().now(); - REQUIRE(estimator->setInput(measurement)); - REQUIRE(estimator->advance()); - auto toc = BipedalLocomotion::clock().now(); +// for (int i = 0; i<700; i++) +// { +// std::cout << "index = " << i << std::endl; + auto tic = BipedalLocomotion::clock().now(); + REQUIRE(estimator->setInput(measurement)); + REQUIRE(estimator->advance()); + auto toc = BipedalLocomotion::clock().now(); - BipedalLocomotion::log()->error("{}", toc - tic); + BipedalLocomotion::log()->error("{}", toc - tic); - RobotDynamicsEstimatorOutput result = estimator->getOutput(); + RobotDynamicsEstimatorOutput result = estimator->getOutput(); +// } } diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/model.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/model.ini index 3b0061a9ea..66a320395c 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/config/model.ini +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/model.ini @@ -10,8 +10,8 @@ associated_joints ("r_leg_ft_sensor", "r_foot_front_ft_sensor", "r_foot_rear_ft_ [ACCELEROMETER] names ("r_leg_ft_acc", "r_foot_front_ft_acc", "r_foot_rear_ft_acc") -frames ("r_leg_ft_sensor", "r_foot_front_ft_sensor", "r_foot_rear_ft_sensor") +frames ("r_leg_ft", "r_foot_front_ft", "r_foot_rear_ft") [GYROSCOPE] names ("r_leg_ft_gyro", "r_foot_front_ft_gyro", "r_foot_rear_ft_gyro") -frames ("r_leg_ft_sensor", "r_foot_front_ft_sensor", "r_foot_rear_ft_sensor") +frames ("r_leg_ft", "r_foot_front_ft", "r_foot_rear_ft") diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini index 8653ef21f0..a708005f91 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini @@ -3,6 +3,8 @@ dynamics_list ("JOINT_VELOCITY", "MOTOR_CURRENT", "RIGHT_LEG_ACC", "RIGHT_FOOT_FRONT_ACC", "RIGHT_FOOT_REAR_ACC", "RIGHT_LEG_GYRO", "RIGHT_FOOT_FRONT_GYRO", "RIGHT_FOOT_REAR_GYRO") +# Available dynamics = ["ZeroVelocityDynamics", "AccelerometerMeasurementDynamics", "GyroscopeMeasurementDynamics", "MotorCurrentMeasurementDynamics"] + [JOINT_VELOCITY] name "ds" elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") @@ -46,14 +48,14 @@ use_bias 1 dynamic_model "AccelerometerMeasurementDynamics" [RIGHT_FOOT_FRONT_ACC] -name "r_leg_ft_acc" +name "r_foot_front_ft_acc" elements ("x", "y", "z") covariance (2.3e-3, 1.9e-3, 3.1e-3) use_bias 1 dynamic_model "AccelerometerMeasurementDynamics" [RIGHT_FOOT_REAR_ACC] -name "r_leg_ft_acc" +name "r_foot_rear_ft_acc" elements ("x", "y", "z") covariance (2.5e-3, 2.3e-3, 3e-3) use_bias 1 @@ -67,14 +69,14 @@ use_bias 1 dynamic_model "GyroscopeMeasurementDynamics" [RIGHT_FOOT_FRONT_GYRO] -name "r_leg_ft_gyro" +name "r_foot_front_ft_gyro" elements ("x", "y", "z") covariance (4.1e-4, 3.3e-4, 3.2e-4) use_bias 1 dynamic_model "GyroscopeMeasurementDynamics" [RIGHT_FOOT_REAR_GYRO] -name "r_leg_ft_gyro" +name "r_foot_rear_ft_gyro" elements ("x", "y", "z") covariance (4.9e-4, 4.2e-4, 3.3e-4) use_bias 1 diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini index 8ce1901184..d182e02031 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini @@ -1,7 +1,9 @@ dynamics_list ("JOINT_VELOCITY", "MOTOR_TORQUE", "FRICTION_TORQUE", "RIGHT_LEG_FT", "RIGHT_FOOT_FRONT_FT", "RIGHT_FOOT_REAR_FT", - "RIGHT_LEG_FT_BIAS", "RIGHT_FOOT_FRONT_FT_BIAS", "RIGHT_FOOT_REAR_FT_BIAS", "RIGHT_LEG_ACC_BIAS", - "RIGHT_FOOT_FRONT_ACC_BIAS", "RIGHT_FOOT_REAR_ACC_BIAS", "RIGHT_LEG_GYRO_BIAS", "RIGHT_FOOT_FRONT_GYRO_BIAS", - "RIGHT_FOOT_REAR_GYRO_BIAS") + "RIGHT_LEG_FT_BIAS", "RIGHT_FOOT_FRONT_FT_BIAS", "RIGHT_FOOT_REAR_FT_BIAS", + "RIGHT_LEG_ACC_BIAS", "RIGHT_FOOT_FRONT_ACC_BIAS", "RIGHT_FOOT_REAR_ACC_BIAS", + "RIGHT_LEG_GYRO_BIAS", "RIGHT_FOOT_FRONT_GYRO_BIAS", "RIGHT_FOOT_REAR_GYRO_BIAS") + +# Available dynamics = ["ZeroVelocityDynamics", "JointVelocityStateDynamics", "FrictionTorqueStateDynamics"] [JOINT_VELOCITY] name "ds" From 895a9e0fca7c3666c00d96a00da36439ee0389d3 Mon Sep 17 00:00:00 2001 From: Ines Date: Fri, 28 Apr 2023 09:38:56 +0200 Subject: [PATCH 13/13] Fix bug --- .../src/AccelerometerMeasurementDynamics.cpp | 15 +++++++++++---- .../src/GyroscopeMeasurementDynamics.cpp | 15 +++++++++++---- src/Estimators/src/UkfMeasurement.cpp | 17 ----------------- 3 files changed, 22 insertions(+), 25 deletions(-) diff --git a/src/Estimators/src/AccelerometerMeasurementDynamics.cpp b/src/Estimators/src/AccelerometerMeasurementDynamics.cpp index c6bb2d7e82..a8f482eca1 100644 --- a/src/Estimators/src/AccelerometerMeasurementDynamics.cpp +++ b/src/Estimators/src/AccelerometerMeasurementDynamics.cpp @@ -69,7 +69,6 @@ bool RDE::AccelerometerMeasurementDynamics::initialize(std::weak_ptr 0) { @@ -301,8 +305,11 @@ bool RDE::AccelerometerMeasurementDynamics::update() void RDE::AccelerometerMeasurementDynamics::setState(const Eigen::Ref ukfState) { - m_bias = ukfState.segment(m_stateVariableHandler.getVariable(m_biasVariableName).offset, - m_stateVariableHandler.getVariable(m_biasVariableName).size); + if (m_useBias) + { + m_bias = ukfState.segment(m_stateVariableHandler.getVariable(m_biasVariableName).offset, + m_stateVariableHandler.getVariable(m_biasVariableName).size); + } } void RDE::AccelerometerMeasurementDynamics::setInput(const UKFInput& ukfInput) diff --git a/src/Estimators/src/GyroscopeMeasurementDynamics.cpp b/src/Estimators/src/GyroscopeMeasurementDynamics.cpp index acae98f028..ba2fee4d4c 100644 --- a/src/Estimators/src/GyroscopeMeasurementDynamics.cpp +++ b/src/Estimators/src/GyroscopeMeasurementDynamics.cpp @@ -69,7 +69,6 @@ bool RDE::GyroscopeMeasurementDynamics::initialize(std::weak_ptrgetGyroscopeJacobian(m_name).block(0, 0, 6, 6) * m_subModelBaseVel.coeffs(); - m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) = m_JvBase.segment(3, 3) + m_bias; + m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) = m_JvBase.segment(3, 3); + + if (m_useBias) + { + m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) += m_bias; + } if (m_subModelList[m_subModelWithGyro[index]].getJointMapping().size() > 0) { @@ -228,8 +232,11 @@ void RDE::GyroscopeMeasurementDynamics::setState(const Eigen::RefmeasurementSize = 0; // finalize all the dynamics -// for (auto& [name, dynamics] : m_pimpl->dynamicsList) -// { -// if(!dynamics->finalize(handler)) -// { -// BipedalLocomotion::log()->error("{} Error while finalizing the dynamics named {}", logPrefix, name); -// return false; -// } - -// m_pimpl->measurementSize += dynamics->size(); -// } for (int indexDyn1 = 0; indexDyn1 < m_pimpl->dynamicsList.size(); indexDyn1++) { if(!m_pimpl->dynamicsList[indexDyn1].second->finalize(handler)) @@ -250,13 +240,6 @@ bool RDE::UkfMeasurement::finalize(const System::VariablesHandler& handler) m_pimpl->covarianceR.resize(m_pimpl->measurementSize, m_pimpl->measurementSize); m_pimpl->covarianceR.setZero(); -// for (auto& [name, dynamics] : m_pimpl->dynamicsList) -// { -// m_pimpl->measurementVariableHandler.addVariable(name, dynamics->getCovariance().size()); - -// m_pimpl->covarianceR.block(m_pimpl->measurementVariableHandler.getVariable(name).offset, m_pimpl->measurementVariableHandler.getVariable(name).offset, -// m_pimpl->measurementVariableHandler.getVariable(name).size, m_pimpl->measurementVariableHandler.getVariable(name).size) = dynamics->getCovariance().asDiagonal(); -// } for (int indexDyn2 = 0; indexDyn2 < m_pimpl->dynamicsList.size(); indexDyn2++) { m_pimpl->measurementVariableHandler.addVariable(m_pimpl->dynamicsList[indexDyn2].first, m_pimpl->dynamicsList[indexDyn2].second->getCovariance().size());