From ac8593c9bd88872e759801595307706d7abf355b Mon Sep 17 00:00:00 2001 From: Ines Date: Thu, 2 Mar 2023 11:34:26 +0100 Subject: [PATCH] Add joint velocity dynamics --- src/Estimators/CMakeLists.txt | 4 + .../FrictionTorqueDynamics.h | 17 +- .../JointVelocityDynamics.h | 112 ++++++++ .../RobotDynamicsEstimator/StateDynamics.h | 20 +- .../RobotDynamicsEstimator/SubModel.h | 66 ++++- .../SubModelKinDynWrapper.h | 27 +- .../ZeroVelocityDynamics.h | 15 +- src/Estimators/src/FrictionTorqueDynamics.cpp | 69 +++-- src/Estimators/src/JointVelocityDynamics.cpp | 256 ++++++++++++++++++ src/Estimators/src/StateDynamics.cpp | 8 +- src/Estimators/src/SubModel.cpp | 153 +++++++---- src/Estimators/src/SubModelKinDynWrapper.cpp | 75 ++--- src/Estimators/src/ZeroVelocityDynamics.cpp | 44 ++- .../RobotDynamicsEstimator/CMakeLists.txt | 40 ++- .../FrictionTorqueDynamicsTest.cpp | 35 ++- .../JointVelocityDynamicsTest.cpp | 208 ++++++++++++++ .../SubModelKinDynWrapperTest.cpp | 10 +- .../ZeroVelocityDynamicsTest.cpp | 24 +- 18 files changed, 967 insertions(+), 216 deletions(-) create mode 100644 src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityDynamics.h create mode 100644 src/Estimators/src/JointVelocityDynamics.cpp create mode 100644 src/Estimators/tests/RobotDynamicsEstimator/JointVelocityDynamicsTest.cpp diff --git a/src/Estimators/CMakeLists.txt b/src/Estimators/CMakeLists.txt index ba086c32fa..84a5525a47 100644 --- a/src/Estimators/CMakeLists.txt +++ b/src/Estimators/CMakeLists.txt @@ -32,15 +32,19 @@ if(FRAMEWORK_COMPILE_RobotDynamicsEstimator) src/StateDynamics.cpp src/ZeroVelocityDynamics.cpp src/FrictionTorqueDynamics.cpp + src/JointVelocityDynamics.cpp SUBDIRECTORIES tests/RobotDynamicsEstimator PUBLIC_HEADERS ${H_PREFIX}/SubModel.h ${H_PREFIX}/SubModelKinDynWrapper.h ${H_PREFIX}/StateDynamics.h ${H_PREFIX}/ZeroVelocityDynamics.h ${H_PREFIX}/FrictionTorqueDynamics.h + ${H_PREFIX}/JointVelocityDynamics.h PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler BipedalLocomotion::TextLogging BipedalLocomotion::ManifConversions + BipedalLocomotion::System + BipedalLocomotion::Math iDynTree::idyntree-high-level iDynTree::idyntree-core iDynTree::idyntree-model diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueDynamics.h index 66c97f1928..f9315eaf2c 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueDynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueDynamics.h @@ -38,7 +38,9 @@ namespace RobotDynamicsEstimator class FrictionTorqueDynamics : public StateDynamics { Eigen::VectorXd m_jointVelocity; /**< 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. */ protected: Eigen::VectorXd m_tanhArgument; @@ -61,10 +63,18 @@ class FrictionTorqueDynamics : public StateDynamics * | `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; + /** + * Set Variablehandler object. + * @param variableHandler object. + * @return True in case of success, false otherwise. + */ + bool setVariableHandler(System::VariablesHandler variableHandler) override; + /** * Update the content of the element. * @return True in case of success, false otherwise. @@ -72,12 +82,11 @@ class FrictionTorqueDynamics : public StateDynamics bool update() override; /** - * Set the state needed to update the dynamics. - * @param state reference to the ukf state. + * Set the state of the ukf needed to update the dynamics. + * @param ukfState reference to the ukf state. * @return true if the current state has been updated correctly. */ - bool setState(const Eigen::Ref jointVelocity, - const Eigen::Ref frictionTorque); + void setState(const Eigen::Ref ukfState) override; }; } // namespace RobotDynamicsEstimator diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityDynamics.h new file mode 100644 index 0000000000..fd3f6547b0 --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityDynamics.h @@ -0,0 +1,112 @@ +/** + * @file JointVelocityDynamics.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_DYNAMICS_H +#define BIPEDAL_LOCOMOTION_ESTIMATORS_JOINT_VELOCITY_DYNAMICS_H + +#include +#include + +#include +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace Estimators +{ +namespace RobotDynamicsEstimator +{ + +/** + * The JointVelocityDynamics class is a concrete implementation of the StateDynamics. + * 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 JointVelocityDynamics : public StateDynamics +{ + std::shared_ptr m_kinDynWrapper; /**< object containing the kinematics and dynamics properties of a sub-model. */ + SubModel m_subModel; /**< Sub-model object storing information about the model (joints, sensors, applied wrenches) */ + std::map m_FTMap; /**< The map containes names of the ft sensors and values of the wrench */ + std::map m_ExtContactMap; /**< The map containes names of the ft sensors and values of the wrench */ + Eigen::VectorXd m_baseVelocity; /**< Velocity of the base of the sub-model. */ + Eigen::VectorXd m_motorTorqueFullModel; /**< Motor torque vector of full-model. */ + Eigen::VectorXd m_frictionTorqueFullModel; /**< Friction torque vector of full-model. */ + Eigen::VectorXd m_jointVelocityFullModel; /**< Joint velocities of full-model. */ + Eigen::VectorXd m_motorTorque; /**< Motor torque vector of sub-model. */ + Eigen::VectorXd m_frictionTorque; /**< Friction torque vector of sub-model. */ + Eigen::VectorXd m_jointVelocity; /**< Joint velocities of sub-model. */ + Eigen::VectorXd m_jointAcceleration; /**< Joint accelerations of sub-model. */ + Eigen::VectorXd m_totalTorqueFromContacts; /**< Joint torques due to known and unknown contacts on the sub-model. */ + Math::Wrenchd m_wrench; /**< Joint torques due to a specific contact. */ + Eigen::VectorXd m_torqueFromContact; /**< Joint torques due to a specific contact. */ + double m_dT; /**< Sampling time. */ + + void computeTotalTorqueFromContacts(); + +public: + /** + * 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); + + /** + * Set the SubModel object. + * @param subModel pointer to a SubModel object. + */ + void setSubModel(SubModel& subModel); + + /** + * 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; + + /** + * Set Variablehandler object. + * @param variableHandler object. + * @return True in case of success, false otherwise. + */ + bool setVariableHandler(System::VariablesHandler variableHandler) 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. + * @return true if the current state has been updated correctly. + */ + void setState(const Eigen::Ref ukfState) override; + +}; + +} // namespace RobotDynamicsEstimator +} // namespace Estimators +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_JOINT_VELOCITY_DYNAMICS_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/StateDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/StateDynamics.h index 38d69e2c2f..ef416ea3d3 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/StateDynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/StateDynamics.h @@ -30,16 +30,15 @@ class StateDynamics { protected: int m_size; /**< Size of the sub-state associate to the StateDynamics object. */ - Eigen::VectorXd m_currentState; /**< Current state computed at the previous step. */ - Eigen::VectorXd m_nextState; /**< Next state computed using the dynamic model. It returns the sub-state associated to the StateDynamics object. */ + Eigen::VectorXd m_updatedState; /**< Next state computed using the dynamic model. It returns the sub-state associated to the StateDynamics object. */ std::string m_description{"Generic State Dynamics Element"}; /**< String describing the content of the task */ std::string m_name; /**< Name of state dynamics. */ Eigen::VectorXd m_covariances; /**< Vector of covariances of the sub-state m_nextState. */ std::vector m_elements = {}; /**< Elements composing the substate m_name. This parameter is optional. */ std::string m_dynamicModel; - double m_dT = 0.0; bool m_isInitialized{false}; /**< True if the state dynamics has been initialized. */ + System::VariablesHandler m_variableHandler; public: /** @@ -50,6 +49,12 @@ class StateDynamics virtual bool initialize(std::weak_ptr paramHandler); + /** + * Set Variablehandler object. + * @param variableHandler object + */ + virtual bool setVariableHandler(System::VariablesHandler variableHandler); + /** * Update the dynamics of the state. * @return True in case of success, false otherwise. @@ -60,7 +65,7 @@ class StateDynamics * Get the next state m_nextState. * @return a const reference to the next state. */ - Eigen::Ref getNextState() const; + Eigen::Ref getUpdatedSubState() const; /** * Get the size of the state. @@ -69,10 +74,11 @@ class StateDynamics int size() const; /** - * Set the sampling time. - * @param samplingTime is the sampling time at which the filter is run. + * Set the state of the ukf needed to update the dynamics. + * @param ukfState reference to the ukf state. + * @return true if the current state has been updated correctly. */ - void setSamplingTime(double samplingTime); + virtual void setState(const Eigen::Ref ukfState) = 0; /** * Destructor. diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h index 3ef1f5c05b..0f07d9f8d6 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h @@ -73,47 +73,101 @@ class SubModel std::vector m_externalContactList; /**< List of the additional external contacts */ public: + /** + * Determines the validity of the object. + * @return True if the object is valid, false otherwise. + */ + bool isValid() const; + /** * Getters */ /** - * @brief Access model. + * @brief Access m_model. * @return The model of the SubModel. */ const iDynTree::Model& getModel() const; /** - * @brief Access jointListMapping. + * @brief Access m_jointListMapping. * @return the mapping between the joint indeces in the sub-model and the joint indeces in the * full-model. */ const std::vector& getJointMapping() const; /** - * @brief Access ftList. + * @brief Access m_ftList. * @return the list of FT objects which is the list of force/torque sensors. */ const std::vector& getFTList() const; /** - * @brief Access accelerometerList. + * @brief Access m_accelerometerList. * @return a list of Sensor objects describing the accelerometers contained in the sub-model. */ const std::vector& getAccelerometerList() const; /** - * @brief Access gyroscopeList. + * @brief Access m_gyroscopeList. * @return a list of Sensor objects describing the gyroscope contained in the sub-model. */ const std::vector& getGyroscopeList() const; /** - * @brief Access externalContactList. + * @brief Access m_externalContactList. * @return a list of strings describing frame names of the external contacts for the sub-model. */ const std::vector& getExternalContactList() const; + /** + * @brief access the length of m_ftList. + * @return the number of force/torque sensors in the sub-model. + */ + int getNrOfFTSensor() const; + + /** + * @brief access the length of m_accelerometerList. + * @return the number of accelerometer sensors in the sub-model. + */ + int getNrOfAccelerometer() const; + + /** + * @brief access the length of m_gyroscopeList. + * @return the number of gyroscope sensors in the sub-model. + */ + int getNrOfGyroscope() const; + + /** + * @brief access the length of m_externalContactList. + * @return the number of gyroscope sensors in the sub-model. + */ + int getNrOfExternalContact() const; + + /** + * @brief Access an element of m_ftList. + * @return FT object associated with the specified index. + */ + const FT& getFTSensor(const int index) const; + + /** + * @brief Access an element of m_accelerometerList. + * @return a Sensor object corresponding to the accelerometer associated with the specified index. + */ + const Sensor& getAccelerometer(const int index) const; + + /** + * @brief Access an element of m_gyroscopeList. + * @return a Sensor object corresponding to the gyroscope associated with the specified index. + */ + const Sensor& getGyroscope(const int index) const; + + /** + * @brief Access an element of m_externalContactList. + * @return a string corresponding to the external contact frame associated with the specified index. + */ + const std::string& getExternalContact(const int index) const; + friend class SubModelCreator; }; diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h index 190eaab278..50e412c1eb 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h @@ -56,6 +56,8 @@ class SubModelKinDynWrapper manif::SE3d::Tangent m_baseVelocity; /**< Velocity of the base of the sub-model */ std::string m_baseFrame; /**< Name of the base frame of the sub-model */ + std::shared_ptr m_kinDynFullModel; + protected: int m_numOfJoints; /**< Number of joints in the sub-model */ Eigen::MatrixXd m_FTranspose; /**< It is the bottom-left block of the mass matrix, that is, @@ -82,22 +84,28 @@ class SubModelKinDynWrapper bool updateDynamicsVariableState(); public: + /** + * @brief set kinDyn + * @param kinDyn is an iDynTree KinDynComputation object handling the kinematics and the + * dynamics of the model. + * @return a boolean value saying if the input pointer is valid + */ + bool setKinDyn(std::shared_ptr kinDyn); + /** * @brief initialize initializes the kindyncomputation object and resizes * all the variable not allocated yet. * @param subModel a struct of type SubModif el describing the subModel structure and model. - * @param kinDynFullModel is the KinDynComputation object associated to a model. * @return a boolean value saying if all the variables are initialized correctly. */ bool - initialize(SubModel& subModel, std::shared_ptr kinDynFullModel); + initialize(SubModel& subModel); /** * @brief updateKinDynState updates the state of the KinDynWrapper object. - * @param kinDynFullModel is the KinDynComputation object associated to a model. * @return a boolean value saying if the subModelList has been created correctly. */ - bool updateKinDynState(std::shared_ptr kinDynFullModel); + bool updateKinDynState(); /** * @brief inverseDynamics computes the free floaing inverse dynamics @@ -116,25 +124,22 @@ class SubModelKinDynWrapper Eigen::Ref jointAcceleration); /** - * @brief getBaseAcceleration gets the acceleration of the sub-model base - * @param kinDynFullModel is the KinDynComputation object associated to a model. + * @brief getBaseAcceleration gets the acceleration of the sub-model base. * @param robotBaseAcceleration the acceleration of the robot base. * @param robotJointAcceleration the acceleration of the robot joints. * @param subModelBaseAcceleration the base acceleration of the sub-model. * @return a boolean value. */ - bool getBaseAcceleration(std::shared_ptr kinDynFullModel, - manif::SE3d::Tangent& robotBaseAcceleration, + bool getBaseAcceleration(manif::SE3d::Tangent& robotBaseAcceleration, Eigen::Ref robotJointAcceleration, manif::SE3d::Tangent& subModelBaseAcceleration); /** - * @brief getBaseVelocity gets the acceleration of the sub-model base - * @param kinDynFullModel is the KinDynComputation object associated to a model. + * @brief getBaseVelocity gets the acceleration of the sub-model base. * @return baseVelocity the the base velocity of the sub-model. */ const manif::SE3d::Tangent& - getBaseVelocity(std::shared_ptr kinDynFullModel); + getBaseVelocity(); /** * Getters diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h index 39dd0bc0ef..eec44fd104 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h @@ -33,6 +33,8 @@ namespace RobotDynamicsEstimator class ZeroVelocityDynamics : public StateDynamics { + Eigen::VectorXd m_currentState; /**< Current state. */ + public: /** * Initialize the state dynamics. @@ -48,6 +50,13 @@ class ZeroVelocityDynamics : public StateDynamics */ bool initialize(std::weak_ptr paramHandler) override; + /** + * Set Variablehandler object. + * @param variableHandler object. + * @return True in case of success, false otherwise. + */ + bool setVariableHandler(System::VariablesHandler variableHandler) override; + /** * Update the content of the element. * @return True in case of success, false otherwise. @@ -55,11 +64,11 @@ class ZeroVelocityDynamics : public StateDynamics bool update() override; /** - * Set the state needed to update the dynamics. - * @param state reference to the ukf state. + * Set the state of the ukf needed to update the dynamics. + * @param ukfState reference to the ukf state. * @return true if the current state has been updated correctly. */ - bool setState(const Eigen::Ref state); + void setState(const Eigen::Ref ukfState) override; }; // class ZeroVelocityDynamics diff --git a/src/Estimators/src/FrictionTorqueDynamics.cpp b/src/Estimators/src/FrictionTorqueDynamics.cpp index ce20733b4c..e208d6c053 100644 --- a/src/Estimators/src/FrictionTorqueDynamics.cpp +++ b/src/Estimators/src/FrictionTorqueDynamics.cpp @@ -70,15 +70,21 @@ bool RDE::FrictionTorqueDynamics::initialize(std::weak_ptrgetParameter("sampling_time", m_dT)) + { + log()->error("{} Error while retrieving the sampling_time variable.", errorPrefix); + return false; + } + m_description = "Friction Torque Dynamics"; m_size = m_covariances.size(); - m_nextState.resize(m_size); - m_nextState.setZero(); + m_currentFrictionTorque.resize(m_size); + m_currentFrictionTorque.setZero(); - m_currentState.resize(m_size); - m_currentState.setZero(); + m_updatedState.resize(m_size); + m_updatedState.setZero(); m_jointVelocity.resize(m_size); m_jointVelocity.setZero(); @@ -98,13 +104,25 @@ bool RDE::FrictionTorqueDynamics::initialize(std::weak_ptrerror("{} The variable handler does not contain the expected state with name `tau_F`.", errorPrefix); + return false; } - else + + if (!m_variableHandler.getVariable("ds").isValid()) { - log()->error("{} Please set the sampling time before initializing the state dynamics.", errorPrefix); + log()->error("{} The variable handler does not contain the expected state with name `ds`.", errorPrefix); return false; } @@ -113,14 +131,6 @@ bool RDE::FrictionTorqueDynamics::initialize(std::weak_ptrinfo("{} The state dynamics is not initialized. Please call initialize method.", errorPrefix); - return false; - } - // k_{1} \dot{s,k} m_tanhArgument = m_k1.array() * m_jointVelocity.array(); @@ -137,31 +147,16 @@ bool RDE::FrictionTorqueDynamics::update() m_dotTauF = m_k2.array() + m_k0k1.array() * m_argParenthesis.array(); // \tau_{F,k+1} = \tau_{F,k} + \Delta T * \dot{\tau_{F,k}} - m_nextState = m_currentState + m_dT * m_dotTauF; + m_updatedState = m_currentFrictionTorque + m_dT * m_dotTauF; return true; } -bool RDE::FrictionTorqueDynamics::setState(const Eigen::Ref jointVelocity, - const Eigen::Ref frictionTorque) +void RDE::FrictionTorqueDynamics::setState(const Eigen::Ref ukfState) { - constexpr auto errorPrefix = "[FrictionTorqueDynamics::setState]"; - - if (frictionTorque.size() != m_currentState.size()) - { - log()->error("{} Wrong frictionTorque size. Expected a vector with size {}.", errorPrefix, m_currentState.size()); - return false; - } + m_jointVelocity = ukfState.segment(m_variableHandler.getVariable("ds").offset, + m_variableHandler.getVariable("ds").size); - if (jointVelocity.size() != m_jointVelocity.size()) - { - log()->error("{} Wrong jointVelocity size. Expected a vector with size {}.", errorPrefix, m_jointVelocity.size()); - return false; - } - - m_jointVelocity = jointVelocity; - - m_currentState = frictionTorque; - - return true; + m_currentFrictionTorque = ukfState.segment(m_variableHandler.getVariable("tau_F").offset, + m_variableHandler.getVariable("tau_m").size); } diff --git a/src/Estimators/src/JointVelocityDynamics.cpp b/src/Estimators/src/JointVelocityDynamics.cpp new file mode 100644 index 0000000000..c3993b64d9 --- /dev/null +++ b/src/Estimators/src/JointVelocityDynamics.cpp @@ -0,0 +1,256 @@ +/** + * @file JointVelocityDynamics.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; + +bool RDE::JointVelocityDynamics::setKinDynWrapper(std::shared_ptr kinDynWrapper) +{ + m_kinDynWrapper = kinDynWrapper; + return (m_kinDynWrapper != nullptr); +} + +void RDE::JointVelocityDynamics::setSubModel(SubModel& subModel) +{ + m_subModel = subModel; +} + +bool RDE::JointVelocityDynamics::setVariableHandler(System::VariablesHandler variableHandler) +{ + constexpr auto errorPrefix = "[JointVelocityDynamics::setVariableHandler]"; + + m_variableHandler = variableHandler; + + if (!m_subModel.isValid()) + { + log()->error("{} Set the subModel before setting the variable handler", errorPrefix); + return false; + } + + if (m_kinDynWrapper == nullptr) + { + log()->error("{} Set the SubModelKynDynWrapper before setting the variable handler", errorPrefix); + return false; + } + + // Check if the variable handler contains the variables used by this dynamics + if (!m_variableHandler.getVariable("tau_m").isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `tau_m`.", errorPrefix); + return false; + } + + if (!m_variableHandler.getVariable("tau_F").isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `tau_F`.", errorPrefix); + return false; + } + + for (int idx = 0; idx < m_subModel.getNrOfFTSensor(); idx++) + { + if (!m_variableHandler.getVariable(m_subModel.getFTSensor(idx).name).isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_subModel.getFTSensor(idx).name); + return false; + } + } + + for (int idx = 0; idx < m_subModel.getNrOfExternalContact(); idx++) + { + if (!m_variableHandler.getVariable(m_subModel.getExternalContact(idx)).isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_subModel.getExternalContact(idx)); + return false; + } + } + + return true; +} + +bool RDE::JointVelocityDynamics::initialize(std::weak_ptr paramHandler) +{ + constexpr auto errorPrefix = "[JointVelocityDynamics::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 = {}; + } + + // Inialize map of the ft sensors + for (int idx = 0; idx < m_subModel.getNrOfFTSensor(); idx++) + { + m_FTMap[m_subModel.getFTSensor(idx).name].setZero(); + } + + // Inialize map of the external contacts + for (int idx = 0; idx < m_subModel.getNrOfExternalContact(); idx++) + { + m_ExtContactMap[m_subModel.getExternalContact(idx)].setZero(); + } + + m_description = "Joint velocity dynamics depending on the robot dynamic model"; + + // Resize and initialize variables + m_baseVelocity.resize(6); + m_baseVelocity.setZero(); + + m_motorTorque.resize(m_subModel.getModel().getNrOfDOFs()); + m_motorTorque.setZero(); + + m_motorTorqueFullModel.resize(m_variableHandler.getVariable("tau_m").size); + m_motorTorqueFullModel.setZero(); + + m_frictionTorque.resize(m_subModel.getModel().getNrOfDOFs()); + m_frictionTorque.setZero(); + + m_frictionTorqueFullModel.resize(m_variableHandler.getVariable("tau_m").size); + m_frictionTorqueFullModel.setZero(); + + m_jointVelocity.resize(m_subModel.getModel().getNrOfDOFs()); + m_jointVelocity.setZero(); + + m_jointVelocityFullModel.resize(m_variableHandler.getVariable("ds").size); + m_jointVelocityFullModel.setZero(); + + m_jointAcceleration.resize(m_subModel.getModel().getNrOfDOFs()); + m_jointAcceleration.setZero(); + + m_totalTorqueFromContacts.resize(m_subModel.getModel().getNrOfDOFs()); + m_totalTorqueFromContacts.setZero(); + + m_torqueFromContact.resize(m_subModel.getModel().getNrOfDOFs()); + m_torqueFromContact.setZero(); + + m_wrench.setZero(); + + return true; +} + +void RDE::JointVelocityDynamics::computeTotalTorqueFromContacts() +{ + m_totalTorqueFromContacts.setZero(); + + // Contribution of FT measurements + for (int idx = 0; idx < m_subModel.getNrOfFTSensor(); idx++) + { + m_wrench = (int)m_subModel.getFTSensor(idx).forceDirection * m_FTMap[m_subModel.getFTSensor(idx).name].array(); + + m_torqueFromContact = m_kinDynWrapper->getFTJacobian(m_subModel.getFTSensor(idx).name).block(0, 6, 6, m_subModel.getModel().getNrOfDOFs()).transpose() * m_wrench; + + m_totalTorqueFromContacts = m_totalTorqueFromContacts.array() + m_torqueFromContact.array(); + } + + // Contribution of unknown external contacts + for (int idx = 0; idx < m_subModel.getNrOfExternalContact(); idx++) + { + m_torqueFromContact = m_kinDynWrapper->getFTJacobian(m_subModel.getFTSensor(idx).name).block(0, 6, 6, m_subModel.getModel().getNrOfDOFs()).transpose() * m_FTMap[m_subModel.getExternalContact(idx)]; + + m_totalTorqueFromContacts = m_totalTorqueFromContacts.array() + m_torqueFromContact.array(); + } +} + +bool RDE::JointVelocityDynamics::update() +{ + constexpr auto errorPrefix = "[JointVelocityDynamics::update]"; + + computeTotalTorqueFromContacts(); + + m_baseVelocity = m_kinDynWrapper->getBaseVelocity().coeffs(); + + if(!m_kinDynWrapper->inverseDynamics(m_motorTorque, + m_frictionTorque, + m_totalTorqueFromContacts, + m_baseVelocity, + m_jointAcceleration)) + { + log()->error("{} Error while computing the inverse dynamics..", errorPrefix); + return false; + } + + m_updatedState = m_jointVelocity + m_dT * m_jointAcceleration; + + return true; +} + +void RDE::JointVelocityDynamics::setState(const Eigen::Ref ukfState) +{ + m_jointVelocityFullModel = ukfState.segment(m_variableHandler.getVariable("ds").offset, + m_variableHandler.getVariable("ds").size); + + for (int idx = 0; idx < m_subModel.getModel().getNrOfDOFs(); idx++) + { + m_jointVelocity(idx) = m_jointVelocityFullModel(m_subModel.getJointMapping().at(idx)); + } + + m_motorTorqueFullModel = ukfState.segment(m_variableHandler.getVariable("tau_m").offset, + m_variableHandler.getVariable("tau_m").size); + + for (int idx = 0; idx < m_subModel.getModel().getNrOfDOFs(); idx++) + { + m_motorTorque(idx) = m_motorTorqueFullModel(m_subModel.getJointMapping().at(idx)); + } + + m_frictionTorqueFullModel = ukfState.segment(m_variableHandler.getVariable("tau_F").offset, + m_variableHandler.getVariable("tau_F").size); + + for (int idx = 0; idx < m_subModel.getModel().getNrOfDOFs(); idx++) + { + m_frictionTorque(idx) = m_frictionTorqueFullModel(m_subModel.getJointMapping().at(idx)); + } + + for (int idx = 0; idx < m_subModel.getNrOfFTSensor(); idx++) + { + m_FTMap[m_subModel.getFTSensor(idx).name] = ukfState.segment(m_variableHandler.getVariable(m_subModel.getFTSensor(idx).name).offset, + m_variableHandler.getVariable(m_subModel.getFTSensor(idx).name).size); + } + + for (int idx = 0; idx < m_subModel.getNrOfExternalContact(); idx++) + { + m_FTMap[m_subModel.getExternalContact(idx)] = ukfState.segment(m_variableHandler.getVariable(m_subModel.getExternalContact(idx)).offset, + m_variableHandler.getVariable(m_subModel.getExternalContact(idx)).size); + } +} diff --git a/src/Estimators/src/StateDynamics.cpp b/src/Estimators/src/StateDynamics.cpp index eb70f05313..64e754af60 100644 --- a/src/Estimators/src/StateDynamics.cpp +++ b/src/Estimators/src/StateDynamics.cpp @@ -21,9 +21,9 @@ bool RDE::StateDynamics::update() return true; } -Eigen::Ref RDE::StateDynamics::getNextState() const +Eigen::Ref RDE::StateDynamics::getUpdatedSubState() const { - return m_nextState; + return m_updatedState; } int RDE::StateDynamics::size() const @@ -31,7 +31,7 @@ int RDE::StateDynamics::size() const return m_size; } -void RDE::StateDynamics::setSamplingTime(double samplingTime) +bool RDE::StateDynamics::setVariableHandler(System::VariablesHandler variableHandler) { - m_dT = samplingTime; + return true; } diff --git a/src/Estimators/src/SubModel.cpp b/src/Estimators/src/SubModel.cpp index 5b3b7331b8..b172db6e9c 100644 --- a/src/Estimators/src/SubModel.cpp +++ b/src/Estimators/src/SubModel.cpp @@ -18,9 +18,14 @@ #include namespace blf = BipedalLocomotion; -namespace blfEstim = BipedalLocomotion::Estimators::RobotDynamicsEstimator; +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; -bool blfEstim::SubModelCreator::splitModel(const std::vector& ftFrameList, +bool RDE::SubModel::isValid() const +{ + return (m_model.getNrOfLinks() > 0); +} + +bool RDE::SubModelCreator::splitModel(const std::vector& ftFrameList, std::vector& idynSubModels) { constexpr auto logPrefix = "[BipedalLocomotion::RobotDynamicsEstimator::SubModelCreator::" @@ -65,10 +70,10 @@ bool blfEstim::SubModelCreator::splitModel(const std::vector& ftFra return true; } -std::vector -blfEstim::SubModelCreator::attachFTsToSubModel(iDynTree::Model& idynSubModel) +std::vector +RDE::SubModelCreator::attachFTsToSubModel(iDynTree::Model& idynSubModel) { - std::vector ftList; + std::vector ftList; for (int ftIdx = 0; ftIdx < this->m_sensorList.getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ftIdx++) @@ -89,17 +94,17 @@ blfEstim::SubModelCreator::attachFTsToSubModel(iDynTree::Model& idynSubModel) if (idynSubModel.getFrameIndex(ftName) >= 0) { - blfEstim::FT ft; + RDE::FT ft; ft.name = ftName; ft.frame = ftName; // Retrieve force direction if (idynSubModel.isLinkNameUsed(linkAppliedWrenchName)) { - ft.forceDirection = blfEstim::FT::Direction::Positive; + ft.forceDirection = RDE::FT::Direction::Positive; } else { - ft.forceDirection = blfEstim::FT::Direction::Negative; + ft.forceDirection = RDE::FT::Direction::Negative; } ftList.push_back(ft); @@ -137,16 +142,16 @@ blfEstim::SubModelCreator::attachFTsToSubModel(iDynTree::Model& idynSubModel) ftName))); } - blfEstim::FT ft; + RDE::FT ft; ft.name = ftName; ft.frame = ftName; if (idynSubModel.isLinkNameUsed(linkAppliedWrenchName)) { - ft.forceDirection = blfEstim::FT::Direction::Positive; + ft.forceDirection = RDE::FT::Direction::Positive; } else { - ft.forceDirection = blfEstim::FT::Direction::Negative; + ft.forceDirection = RDE::FT::Direction::Negative; } ftList.push_back(std::move(ft)); @@ -157,16 +162,16 @@ blfEstim::SubModelCreator::attachFTsToSubModel(iDynTree::Model& idynSubModel) return ftList; } -std::vector blfEstim::SubModelCreator::attachAccelerometersToSubModel( - const std::vector& accListFromConfig, const iDynTree::Model& subModel) +std::vector RDE::SubModelCreator::attachAccelerometersToSubModel( + const std::vector& accListFromConfig, const iDynTree::Model& subModel) { - std::vector accList; + std::vector accList; for (int idx = 0; idx < accListFromConfig.size(); idx++) { if (subModel.isFrameNameUsed(accListFromConfig[idx].frame)) { - blfEstim::Sensor acc; + RDE::Sensor acc; acc.name = accListFromConfig[idx].name; acc.frame = accListFromConfig[idx].frame; accList.push_back(std::move(acc)); @@ -176,16 +181,16 @@ std::vector blfEstim::SubModelCreator::attachAccelerometersToS return accList; } -std::vector blfEstim::SubModelCreator::attachGyroscopesToSubModel( - const std::vector& gyroListFromConfig, const iDynTree::Model& subModel) +std::vector RDE::SubModelCreator::attachGyroscopesToSubModel( + const std::vector& gyroListFromConfig, const iDynTree::Model& subModel) { - std::vector gyroList; + std::vector gyroList; for (int idx = 0; idx < gyroListFromConfig.size(); idx++) { if (subModel.isFrameNameUsed(gyroListFromConfig[idx].frame)) { - blfEstim::Sensor gyro; + RDE::Sensor gyro; gyro.name = gyroListFromConfig[idx].name; gyro.frame = gyroListFromConfig[idx].frame; gyroList.push_back(std::move(gyro)); @@ -195,7 +200,7 @@ std::vector blfEstim::SubModelCreator::attachGyroscopesToSubMo return gyroList; } -std::vector blfEstim::SubModelCreator::attachExternalContactsToSubModel( +std::vector RDE::SubModelCreator::attachExternalContactsToSubModel( const std::vector& contactsFromConfig, const iDynTree::Model& subModel) { std::vector contactList; @@ -211,7 +216,7 @@ std::vector blfEstim::SubModelCreator::attachExternalContactsToSubM return contactList; } -std::vector blfEstim::SubModelCreator::createJointMapping(const iDynTree::Model& subModel) +std::vector RDE::SubModelCreator::createJointMapping(const iDynTree::Model& subModel) { std::vector jointListMapping; @@ -223,36 +228,36 @@ std::vector blfEstim::SubModelCreator::createJointMapping(const iDynTree::M return jointListMapping; } -blfEstim::SubModel -blfEstim::SubModelCreator::populateSubModel(iDynTree::Model& idynSubModel, - const std::vector& accList, - const std::vector& gyroList, +RDE::SubModel +RDE::SubModelCreator::populateSubModel(iDynTree::Model& idynSubModel, + const std::vector& accList, + const std::vector& gyroList, const std::vector& externalContacts) { - blfEstim::SubModel subModel; + RDE::SubModel subModel; - subModel.m_ftList = blfEstim::SubModelCreator::attachFTsToSubModel(idynSubModel); + subModel.m_ftList = RDE::SubModelCreator::attachFTsToSubModel(idynSubModel); subModel.m_model = idynSubModel.copy(); subModel.m_accelerometerList - = blfEstim::SubModelCreator::attachAccelerometersToSubModel(accList, idynSubModel); + = RDE::SubModelCreator::attachAccelerometersToSubModel(accList, idynSubModel); subModel.m_gyroscopeList - = blfEstim::SubModelCreator::attachGyroscopesToSubModel(gyroList, idynSubModel); + = RDE::SubModelCreator::attachGyroscopesToSubModel(gyroList, idynSubModel); subModel.m_externalContactList - = blfEstim::SubModelCreator::attachExternalContactsToSubModel(externalContacts, + = RDE::SubModelCreator::attachExternalContactsToSubModel(externalContacts, idynSubModel); - subModel.m_jointListMapping = blfEstim::SubModelCreator::createJointMapping(idynSubModel); + subModel.m_jointListMapping = RDE::SubModelCreator::createJointMapping(idynSubModel); return subModel; } -bool blfEstim::SubModelCreator::createSubModels(const std::vector& ftSensorList, - const std::vector& accList, - const std::vector& gyroList, +bool RDE::SubModelCreator::createSubModels(const std::vector& ftSensorList, + const std::vector& accList, + const std::vector& gyroList, const std::vector& externalContacts) { constexpr auto logPrefix = "[BipedalLocomotion::RobotDynamicsEstimator::SubModelCreator::" @@ -275,7 +280,7 @@ bool blfEstim::SubModelCreator::createSubModels(const std::vectorm_subModelList.emplace_back( - blfEstim::SubModelCreator::populateSubModel(idynSubModels[idxSubModel], + RDE::SubModelCreator::populateSubModel(idynSubModels[idxSubModel], accList, gyroList, externalContacts)); @@ -284,7 +289,7 @@ bool blfEstim::SubModelCreator::createSubModels(const std::vector parameterHandler) { constexpr auto logPrefix = "[BipedalLocomotion::Estimators::RobotDynamicsEstimator::" @@ -347,10 +352,10 @@ bool blfEstim::SubModelCreator::createSubModels( std::vector ftNames, ftFrames; bool ok = populateSensorParameters("FT", ftNames, ftFrames); - std::vector ftList; + std::vector ftList; for (auto idx = 0; idx < ftNames.size(); idx++) { - blfEstim::Sensor ft; + RDE::Sensor ft; ft.name = ftNames[idx]; ft.frame = ftFrames[idx]; ftList.push_back(std::move(ft)); @@ -359,10 +364,10 @@ bool blfEstim::SubModelCreator::createSubModels( std::vector accNames, accFrames; ok = ok && populateSensorParameters("ACCELEROMETER", accNames, accFrames); - std::vector accList; + std::vector accList; for (auto idx = 0; idx < accNames.size(); idx++) { - blfEstim::Sensor acc; + RDE::Sensor acc; acc.name = accNames[idx]; acc.frame = accFrames[idx]; accList.push_back(std::move(acc)); @@ -371,27 +376,27 @@ bool blfEstim::SubModelCreator::createSubModels( std::vector gyroNames, gyroFrames; ok = ok && populateSensorParameters("GYROSCOPE", gyroNames, gyroFrames); - std::vector gyroList; + std::vector gyroList; for (auto idx = 0; idx < gyroNames.size(); idx++) { - blfEstim::Sensor gyro; + RDE::Sensor gyro; gyro.name = gyroNames[idx]; gyro.frame = gyroFrames[idx]; gyroList.push_back(std::move(gyro)); } ok = ok - && blfEstim::SubModelCreator::createSubModels(ftList, accList, gyroList, extContactFrames); + && RDE::SubModelCreator::createSubModels(ftList, accList, gyroList, extContactFrames); return ok; } -void blfEstim::SubModelCreator::setModel(const iDynTree::Model& model) +void RDE::SubModelCreator::setModel(const iDynTree::Model& model) { this->m_model = model; } -bool blfEstim::SubModelCreator::setKinDyn(std::shared_ptr kinDyn) +bool RDE::SubModelCreator::setKinDyn(std::shared_ptr kinDyn) { if ((kinDyn == nullptr) || (!kinDyn->isValid())) { @@ -404,47 +409,87 @@ bool blfEstim::SubModelCreator::setKinDyn(std::shared_ptrm_sensorList = sensorList; } -const std::vector& blfEstim::SubModelCreator::getSubModelList() const +const std::vector& RDE::SubModelCreator::getSubModelList() const { return this->m_subModelList; } -const blfEstim::SubModel& blfEstim::SubModelCreator::getSubModel(int index) const +const RDE::SubModel& RDE::SubModelCreator::getSubModel(int index) const { return this->m_subModelList.at(index); } -const iDynTree::Model& blfEstim::SubModel::getModel() const +const iDynTree::Model& RDE::SubModel::getModel() const { return this->m_model; } -const std::vector& blfEstim::SubModel::getJointMapping() const +const std::vector& RDE::SubModel::getJointMapping() const { return this->m_jointListMapping; } -const std::vector& blfEstim::SubModel::getFTList() const +const std::vector& RDE::SubModel::getFTList() const { return this->m_ftList; } -const std::vector& blfEstim::SubModel::getAccelerometerList() const +const std::vector& RDE::SubModel::getAccelerometerList() const { return this->m_accelerometerList; } -const std::vector& blfEstim::SubModel::getGyroscopeList() const +const std::vector& RDE::SubModel::getGyroscopeList() const { return this->m_gyroscopeList; } -const std::vector& blfEstim::SubModel::getExternalContactList() const +const std::vector& RDE::SubModel::getExternalContactList() const { return this->m_externalContactList; } + +int RDE::SubModel::getNrOfFTSensor() const +{ + return m_ftList.size(); +} + +int RDE::SubModel::getNrOfAccelerometer() const +{ + return m_accelerometerList.size(); +} + +int RDE::SubModel::getNrOfGyroscope() const +{ + return m_gyroscopeList.size(); +} + +int RDE::SubModel::getNrOfExternalContact() const +{ + return m_externalContactList.size(); +} + +const RDE::FT& RDE::SubModel::getFTSensor(const int index) const +{ + return m_ftList.at(index); +} + +const RDE::Sensor& RDE::SubModel::getAccelerometer(const int index) const +{ + return m_accelerometerList.at(index); +} + +const RDE::Sensor& RDE::SubModel::getGyroscope(const int index) const +{ + return m_gyroscopeList.at(index); +} + +const std::string& RDE::SubModel::getExternalContact(const int index) const +{ + return m_externalContactList.at(index); +} diff --git a/src/Estimators/src/SubModelKinDynWrapper.cpp b/src/Estimators/src/SubModelKinDynWrapper.cpp index a21ba7f914..bc4fd907bc 100644 --- a/src/Estimators/src/SubModelKinDynWrapper.cpp +++ b/src/Estimators/src/SubModelKinDynWrapper.cpp @@ -18,8 +18,20 @@ namespace blf = BipedalLocomotion; namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; -bool RDE::SubModelKinDynWrapper::initialize( - RDE::SubModel& subModel, std::shared_ptr kinDynFullModel) +bool RDE::SubModelKinDynWrapper::setKinDyn(std::shared_ptr kinDyn) +{ + if ((kinDyn == nullptr) || (!kinDyn->isValid())) + { + log()->error("[SubModelKinDynWrapper::setKinDyn] Invalid kinDyn object."); + return false; + } + + this->m_kinDynFullModel = kinDyn; + + return true; +} + +bool RDE::SubModelKinDynWrapper::initialize(RDE::SubModel& subModel) { constexpr auto logPrefix = "[BipedalLocomotion::Estimators::SubModelKinDynWrapper::initialize]"; @@ -38,8 +50,8 @@ bool RDE::SubModelKinDynWrapper::initialize( m_baseVelocity.setZero(); - m_jointPositionModel.resize(kinDynFullModel->getNrOfDegreesOfFreedom()); - m_jointVelocityModel.resize(kinDynFullModel->getNrOfDegreesOfFreedom()); + m_jointPositionModel.resize(m_kinDynFullModel->getNrOfDegreesOfFreedom()); + m_jointVelocityModel.resize(m_kinDynFullModel->getNrOfDegreesOfFreedom()); m_qj.resize(m_numOfJoints); m_dqj.resize(m_numOfJoints); @@ -98,20 +110,19 @@ bool RDE::SubModelKinDynWrapper::initialize( return true; } -bool RDE::SubModelKinDynWrapper::updateKinDynState( - std::shared_ptr kinDynFullModel) +bool RDE::SubModelKinDynWrapper::updateKinDynState() { constexpr auto logPrefix = "[BipedalLocomotion::Estimators::SubModelKinDynWrapper::" "updateKinDynState]"; // Get world transform for the base frame - m_worldTBase = blf::Conversions::toManifPose(kinDynFullModel->getWorldTransform(m_baseFrame)); + m_worldTBase = blf::Conversions::toManifPose(m_kinDynFullModel->getWorldTransform(m_baseFrame)); // Get base velocity - m_baseVelocity = blf::Conversions::toManifTwist(kinDynFullModel->getFrameVel(m_baseFrame)); + m_baseVelocity = blf::Conversions::toManifTwist(m_kinDynFullModel->getFrameVel(m_baseFrame)); // Get position and velocity of the joints in the full model - kinDynFullModel->getRobotState(m_jointPositionModel, m_jointVelocityModel, m_worldGravity); + m_kinDynFullModel->getRobotState(m_jointPositionModel, m_jointVelocityModel, m_worldGravity); // Get positions and velocities of joints of the sub model for (int idx = 0; idx < m_subModel.getJointMapping().size(); idx++) @@ -146,7 +157,7 @@ bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState() } if (!m_kinDyn.generalizedBiasForces( - iDynTree::make_span(m_genForces.data(), m_genForces.size()))) + iDynTree::make_span(m_genForces.data(), m_genForces.size()))) { blf::log()->error("{} Unable to get the generalized bias forces of the sub-model.", logPrefix); @@ -173,9 +184,9 @@ bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState() { // Update jacobian if (!m_kinDyn - .getFrameFreeFloatingJacobian(m_subModel.getAccelerometerList()[idx].frame, - m_jacAccList[m_subModel.getAccelerometerList()[idx] - .name])) + .getFrameFreeFloatingJacobian(m_subModel.getAccelerometerList()[idx].frame, + m_jacAccList[m_subModel.getAccelerometerList()[idx] + .name])) { blf::log()->error("{} Unable to get the compute the free floating jacobian of the " "frame {}.", @@ -186,13 +197,13 @@ bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState() // Update dJnu m_dJnuList[m_subModel.getAccelerometerList()[idx].name] = iDynTree::toEigen( - m_kinDyn.getFrameBiasAcc(m_subModel.getAccelerometerList()[idx].frame)); + m_kinDyn.getFrameBiasAcc(m_subModel.getAccelerometerList()[idx].frame)); // Update rotMatrix m_accRworldList[m_subModel.getAccelerometerList()[idx].name] = blf::Conversions::toManifRot( - m_kinDyn.getWorldTransform(m_subModel.getAccelerometerList()[idx].frame) - .getRotation() - .inverse()); + m_kinDyn.getWorldTransform(m_subModel.getAccelerometerList()[idx].frame) + .getRotation() + .inverse()); } // Update gyroscope jacobians @@ -200,7 +211,7 @@ bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState() { if (!m_kinDyn.getFrameFreeFloatingJacobian(m_subModel.getGyroscopeList()[idx].frame, m_jacGyroList[m_subModel.getGyroscopeList()[idx] - .name])) + .name])) { blf::log()->error("{} Unable to get the compute the free floating jacobian of the " "frame {}.", @@ -215,7 +226,7 @@ bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState() { if (!m_kinDyn.getFrameFreeFloatingJacobian(m_subModel.getExternalContactList()[idx], m_jacContactList - [m_subModel.getExternalContactList()[idx]])) + [m_subModel.getExternalContactList()[idx]])) { blf::log()->error("{} Unable to get the compute the free floating jacobian of the " "frame {}.", @@ -243,7 +254,7 @@ bool RDE::SubModelKinDynWrapper::inverseDynamics(Eigen::Ref mot m_FTBaseAcc = m_FTranspose * baseAcceleration; m_totalTorques - = motorTorqueAfterGearbox - frictionTorques - m_genBiasJointTorques + tauExt + m_FTBaseAcc; + = motorTorqueAfterGearbox - frictionTorques - m_genBiasJointTorques + tauExt + m_FTBaseAcc; jointAcceleration = m_pseudoInverseH * m_totalTorques; @@ -251,20 +262,19 @@ bool RDE::SubModelKinDynWrapper::inverseDynamics(Eigen::Ref mot } bool RDE::SubModelKinDynWrapper::getBaseAcceleration( - std::shared_ptr kinDynFullModel, - manif::SE3d::Tangent& robotBaseAcceleration, - Eigen::Ref robotJointAcceleration, - manif::SE3d::Tangent& subModelBaseAcceleration) + manif::SE3d::Tangent& robotBaseAcceleration, + Eigen::Ref robotJointAcceleration, + manif::SE3d::Tangent& subModelBaseAcceleration) { constexpr auto logPrefix = "[BipedalLocomotion::Estimators::SubModelKinDynWrapper::" "getBaseAcceleration]"; - if (!kinDynFullModel->getFrameAcc(m_baseFrame, - iDynTree::make_span(robotBaseAcceleration.data(), - manif::SE3d::Tangent::DoF), - robotJointAcceleration, - iDynTree::make_span(subModelBaseAcceleration.data(), - manif::SE3d::Tangent::DoF))) + if (!m_kinDynFullModel->getFrameAcc(m_baseFrame, + iDynTree::make_span(robotBaseAcceleration.data(), + manif::SE3d::Tangent::DoF), + robotJointAcceleration, + iDynTree::make_span(subModelBaseAcceleration.data(), + manif::SE3d::Tangent::DoF))) { blf::log()->error("{} Unable to get the acceleration of the frame {}.", logPrefix, @@ -275,10 +285,9 @@ bool RDE::SubModelKinDynWrapper::getBaseAcceleration( return true; } -const manif::SE3d::Tangent& RDE::SubModelKinDynWrapper::getBaseVelocity( - std::shared_ptr kinDynFullModel) +const manif::SE3d::Tangent& RDE::SubModelKinDynWrapper::getBaseVelocity() { - m_baseVelocity = blf::Conversions::toManifTwist(kinDynFullModel->getFrameVel(m_baseFrame)); + m_baseVelocity = blf::Conversions::toManifTwist(m_kinDynFullModel->getFrameVel(m_baseFrame)); return m_baseVelocity; } diff --git a/src/Estimators/src/ZeroVelocityDynamics.cpp b/src/Estimators/src/ZeroVelocityDynamics.cpp index ccfb6b2199..37417d29f3 100644 --- a/src/Estimators/src/ZeroVelocityDynamics.cpp +++ b/src/Estimators/src/ZeroVelocityDynamics.cpp @@ -53,51 +53,41 @@ bool RDE::ZeroVelocityDynamics::initialize(std::weak_ptrerror("{} Please set the sampling time before initializing the state dynamics.", errorPrefix); - return false; - } + m_updatedState.resize(m_size); + m_updatedState.setZero(); return true; } -bool RDE::ZeroVelocityDynamics::update() +bool RDE::ZeroVelocityDynamics::setVariableHandler(System::VariablesHandler variableHandler) { - constexpr auto errorPrefix = "[ZeroVelocityDynamics::update]"; + constexpr auto errorPrefix = "[ZeroVelocityDynamics::setVariableHandler]"; - if (!m_isInitialized) + m_variableHandler = variableHandler; + + // Check if the variable handler contains the variables used by this dynamics + if (!m_variableHandler.getVariable(m_name).isValid()) { - log()->info("{} The state dynamics is not initialized. Please call initialize method.", errorPrefix); + log()->error("{} The variable handler does not contain the expected state name {}.", errorPrefix, m_name); return false; } - m_nextState = m_currentState; - return true; } -bool RDE::ZeroVelocityDynamics::setState(const Eigen::Ref state) +bool RDE::ZeroVelocityDynamics::update() { - constexpr auto errorPrefix = "[ZeroVelocityDynamics::setState]"; - - if (state.size() != m_currentState.size()) - { - log()->error("{} Wrong state size. Expected a vector with size {}.", errorPrefix, m_currentState.size()); - return false; - } - m_currentState = state; + m_updatedState = m_currentState; return true; } + +void RDE::ZeroVelocityDynamics::setState(const Eigen::Ref ukfState) +{ + m_currentState = ukfState.segment(m_variableHandler.getVariable(m_name).offset, + m_variableHandler.getVariable(m_name).size); +} diff --git a/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt b/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt index 2756629b9d..e9bdf4327d 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt +++ b/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt @@ -23,22 +23,34 @@ if(FRAMEWORK_USE_icub-models AND FRAMEWORK_COMPILE_YarpImplementation) ) add_bipedal_test(NAME ZeroVelocityDynamics - SOURCES ZeroVelocityDynamicsTest.cpp - LINKS BipedalLocomotion::RobotDynamicsEstimator - BipedalLocomotion::ParametersHandler - BipedalLocomotion::ParametersHandlerYarpImplementation - icub-models::icub-models - Eigen3::Eigen - ) + SOURCES ZeroVelocityDynamicsTest.cpp + LINKS BipedalLocomotion::RobotDynamicsEstimator + BipedalLocomotion::ParametersHandler + BipedalLocomotion::ParametersHandlerYarpImplementation + icub-models::icub-models + Eigen3::Eigen + ) add_bipedal_test(NAME FrictionTorqueDynamicsTest - SOURCES FrictionTorqueDynamicsTest.cpp - LINKS BipedalLocomotion::RobotDynamicsEstimator - BipedalLocomotion::ParametersHandler - BipedalLocomotion::ParametersHandlerYarpImplementation - icub-models::icub-models - Eigen3::Eigen - ) + SOURCES FrictionTorqueDynamicsTest.cpp + LINKS BipedalLocomotion::RobotDynamicsEstimator + BipedalLocomotion::ParametersHandler + BipedalLocomotion::ParametersHandlerYarpImplementation + BipedalLocomotion::System + icub-models::icub-models + Eigen3::Eigen + ) + + add_bipedal_test(NAME JointVelocityDynamicsTest + SOURCES JointVelocityDynamicsTest.cpp + LINKS BipedalLocomotion::RobotDynamicsEstimator + BipedalLocomotion::ParametersHandler + BipedalLocomotion::ParametersHandlerYarpImplementation + BipedalLocomotion::System + icub-models::icub-models + Eigen3::Eigen + MANIF::manif + ) 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/FrictionTorqueDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueDynamicsTest.cpp index f8279b978d..55e3dea617 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueDynamicsTest.cpp @@ -1,4 +1,4 @@ -/** +/** * @file FrictionTorqueDynamicsTest.cpp * @authors Ines Sorrentino * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and @@ -14,6 +14,7 @@ #include #include #include +#include #include @@ -26,8 +27,9 @@ double random_double() return 1.0 * ((double)rand() - RAND_MAX / 2) / ((double)RAND_MAX); } -TEST_CASE("Zero Velocity Dynamics") +TEST_CASE("Friction Torque Dynamics") { + // Create parameter handler auto parameterHandler = std::make_shared(); const std::string name = "tau_F"; @@ -41,6 +43,7 @@ TEST_CASE("Zero Velocity Dynamics") 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); @@ -49,10 +52,20 @@ TEST_CASE("Zero Velocity Dynamics") 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)); + REQUIRE(variableHandler.addVariable("r_leg_ft_sensor", sizeVariable)); + REQUIRE(variableHandler.addVariable("r_foot_front_ft_sensor", sizeVariable)); FrictionTorqueDynamics tau_F_dynamics; - tau_F_dynamics.setSamplingTime(0.01); + REQUIRE(tau_F_dynamics.setVariableHandler(variableHandler)); REQUIRE(tau_F_dynamics.initialize(parameterHandler)); @@ -68,16 +81,24 @@ TEST_CASE("Zero Velocity Dynamics") jointVel(index) = random_double() * 100; } - REQUIRE(tau_F_dynamics.setState(jointVel, currentStateTauF)); + Eigen::VectorXd state; + state.resize(30); + state.setZero(); + state.segment(variableHandler.getVariable("ds").offset, variableHandler.getVariable("ds").size) = jointVel; + state.segment(variableHandler.getVariable("tau_F").offset, variableHandler.getVariable("tau_F").size) = currentStateTauF; + + tau_F_dynamics.setState(state); REQUIRE(tau_F_dynamics.update()); Eigen::VectorXd nextState(covariance.size()); - nextState = tau_F_dynamics.getNextState(); + nextState = tau_F_dynamics.getUpdatedSubState(); + + Eigen::VectorXd m_tanhArgument = k1.array() * jointVel.array(); + Eigen::VectorXd expectedState = currentStateTauF.array() + dT * (k2.array() + k0.array() * k1.array() * (1 - m_tanhArgument.array().tanh().square())); for (int i = 0; i < nextState.size(); i++) { - bool res = nextState(i) >= -30.0 && nextState(i) <= 30.0; // Acceptable values of friction torques - REQUIRE(res); + REQUIRE(expectedState(i) == nextState(i)); } } diff --git a/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityDynamicsTest.cpp new file mode 100644 index 0000000000..ce430e2a88 --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityDynamicsTest.cpp @@ -0,0 +1,208 @@ +/** + * @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 + +using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; + +double random_double() +{ + return 1.0 * ((double)rand() - RAND_MAX / 2) / ((double)RAND_MAX); +} + +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 createSubModel(iDynTree::ModelLoader& mdlLdr, + std::shared_ptr kinDynFullModel, + IParametersHandler::shared_ptr group, + SubModelCreator& subModelCreator) +{ + subModelCreator.setModel(mdlLdr.model()); + REQUIRE(subModelCreator.setKinDyn(kinDynFullModel)); + subModelCreator.setSensorList(mdlLdr.sensors()); + + 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) = -9.80665; + + 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 = "tau_F"; + Eigen::VectorXd covariance(6); + covariance << 1e-3, 1e-3, 5e-3, 5e-3, 5e-3, 5e-3; + const std::string model = "ZeroVelocityDynamics"; + 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)); + + SubModelCreator subModelCreator; + createSubModel(modelLoader, kinDyn, handlerFromConfig, subModelCreator); + + std::vector> kinDynWrapperList; + 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)->updateKinDynState()); + } + + JointVelocityDynamics ds0; + ds0.setSubModel(subModelList.at(0)); + REQUIRE(ds0.setKinDynWrapper(kinDynWrapperList.at(0))); + REQUIRE(ds0.setVariableHandler(variableHandler)); + REQUIRE(ds0.initialize(parameterHandler)); + + Eigen::VectorXd state; + state.resize(30); + for (int index = 0; index < state.size(); index++) + { + state(index) = random_double() * 100; + } + + REQUIRE(ds0.update()); + + std::cout << "Updated state" << std::endl; + std::cout << ds0.getUpdatedSubState() << std::endl; +} diff --git a/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp index bb1e29c6f8..a9cba4fd0f 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp @@ -141,9 +141,10 @@ TEST_CASE("SubModelKinDynWrapper") for (int idx = 0; idx < subModelList.size(); idx++) { RDE::SubModelKinDynWrapper kinDynSubModel; - REQUIRE(kinDynSubModel.initialize(subModelList[idx], kinDyn)); + REQUIRE(kinDynSubModel.setKinDyn(kinDyn)); + REQUIRE(kinDynSubModel.initialize(subModelList[idx])); - REQUIRE(kinDynSubModel.updateKinDynState(kinDyn)); + REQUIRE(kinDynSubModel.updateKinDynState()); int numberOfJoints = subModelList[idx].getModel().getNrOfDOFs(); @@ -156,8 +157,7 @@ TEST_CASE("SubModelKinDynWrapper") const std::string baseFrame = kinDynSubModel.getBaseFrameName(); manif::SE3d::Tangent baseAcceleration; - REQUIRE(kinDynSubModel.getBaseAcceleration(kinDyn, - robotBaseAcceleration, + REQUIRE(kinDynSubModel.getBaseAcceleration(robotBaseAcceleration, robotJointAcceleration, baseAcceleration)); manif::SE3d::Tangent subModelBaseAcceleration; @@ -170,7 +170,7 @@ TEST_CASE("SubModelKinDynWrapper") REQUIRE(subModelBaseAcceleration == baseAcceleration); manif::SE3d::Tangent baseVelocity; - baseVelocity = kinDynSubModel.getBaseVelocity(kinDyn); + baseVelocity = kinDynSubModel.getBaseVelocity(); manif::SE3d::Tangent baseVelFromFullModel = BipedalLocomotion::Conversions::toManifTwist(kinDyn->getFrameVel(baseFrame)); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityDynamicsTest.cpp index fafd0f4068..be29b78ed5 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityDynamicsTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityDynamicsTest.cpp @@ -38,25 +38,41 @@ TEST_CASE("Zero Velocity Dynamics") parameterHandler->setParameter("name", name); parameterHandler->setParameter("covariance", covariance); parameterHandler->setParameter("dynamic_model", model); + parameterHandler->setParameter("sampling_time", 0.01); - ZeroVelocityDynamics tau_m; + // 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_foot_front_ft_sensor", sizeVariable)); - tau_m.setSamplingTime(0.01); + ZeroVelocityDynamics tau_m; REQUIRE(tau_m.initialize(parameterHandler)); + REQUIRE(tau_m.setVariableHandler(variableHandler)); + Eigen::VectorXd currentState(covariance.size()); for (int index = 0; index < currentState.size(); index++) { currentState(index) = random_double(); } - REQUIRE(tau_m.setState(currentState)); + + Eigen::VectorXd state; + state.resize(30); + state.setZero(); + state.segment(variableHandler.getVariable("tau_m").offset, variableHandler.getVariable("ds").size) = currentState; + + tau_m.setState(state); REQUIRE(tau_m.update()); Eigen::VectorXd nextState(covariance.size()); - nextState = tau_m.getNextState(); + nextState = tau_m.getUpdatedSubState(); REQUIRE(currentState == nextState); }