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..4e5ac401a2 --- /dev/null +++ b/src/Estimators/src/UkfState.cpp @@ -0,0 +1,329 @@ +/** + * @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}; + + bfl::VectorDescription state_description_; + + 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 m_pimpl->state_description_; +} + +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" +