From d183d30d097ba17c807dab26b538c7c5aa025df2 Mon Sep 17 00:00:00 2001 From: Ines Date: Wed, 26 Apr 2023 16:58:18 +0200 Subject: [PATCH] Fix bugs. --- .../AccelerometerMeasurementDynamics.h | 4 +- .../SubModelKinDynWrapper.h | 2 +- .../src/AccelerometerMeasurementDynamics.cpp | 30 +++- src/Estimators/src/SubModelKinDynWrapper.cpp | 8 +- src/Estimators/src/UkfMeasurement.cpp | 135 +++++++++++++----- src/Estimators/src/UkfState.cpp | 92 ++++++++---- src/Estimators/src/ZeroVelocityDynamics.cpp | 1 - .../AccelerometerMeasurementDynamicsTest.cpp | 2 +- .../RobotDynamicsEstimatorTest.cpp | 68 +++++---- .../RobotDynamicsEstimator/config/model.ini | 4 +- .../config/ukf/ukf_measurement.ini | 10 +- .../config/ukf/ukf_state.ini | 8 +- 12 files changed, 253 insertions(+), 111 deletions(-) diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h index 843a8e2358..e1afeeebf5 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h @@ -54,8 +54,8 @@ class AccelerometerMeasurementDynamics : public Dynamics Eigen::VectorXd m_Jsdotdot; Eigen::Vector3d m_accRg; Eigen::Vector3d m_vCrossW; - Eigen::Vector3d linVel; - Eigen::Vector3d angVel; + Eigen::Vector3d m_linVel; + Eigen::Vector3d m_angVel; public: /* diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h index 3d3f734165..723be4feec 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h @@ -85,7 +85,7 @@ class SubModelKinDynWrapper * updateDynamicsVariableState updates the value of all the member variables containing * information about the robot kinematics and dynamics */ - bool updateDynamicsVariableState(bool isCorrectStep); + bool updateDynamicsVariableState(bool updateRobotDynamicsOnly); /** * @brief Compute the contribution of external contacts on the joint torques. diff --git a/src/Estimators/src/AccelerometerMeasurementDynamics.cpp b/src/Estimators/src/AccelerometerMeasurementDynamics.cpp index 403f802515..c6bb2d7e82 100644 --- a/src/Estimators/src/AccelerometerMeasurementDynamics.cpp +++ b/src/Estimators/src/AccelerometerMeasurementDynamics.cpp @@ -150,6 +150,9 @@ bool RDE::AccelerometerMeasurementDynamics::finalize(const System::VariablesHand m_accFrameVel.setZero(); + m_linVel.setZero(); + m_angVel.setZero(); + return true; } @@ -248,17 +251,38 @@ bool RDE::AccelerometerMeasurementDynamics::update() // J_dot nu + base_J v_dot_base + joint_J s_dotdot - acc_Rot_world gravity + bias for (int index = 0; index < m_subModelsWithAccelerometer.size(); index++) { +// std::cout << "------------------------------------------------------- Submodel index " << m_subModelsWithAccelerometer[index] << ", acc name " << m_name << std::endl; + m_JdotNu = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerBiasAcceleration(m_name); +// std::cout << "m_JdotNu" << std::endl; +// std::cout << m_JdotNu << std::endl; + m_JvdotBase = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerJacobian(m_name).block(0, 0, 6, 6) * m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getBaseAcceleration().coeffs(); +// std::cout << "m_JvdotBase" << std::endl; +// std::cout << m_JvdotBase << std::endl; + m_accRg = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerRotation(m_name).rotation() * m_gravity; - linVel = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerVelocity(m_name).coeffs().segment(0, 3); - angVel = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerVelocity(m_name).coeffs().segment(3, 6); +// std::cout << "m_accRg" << std::endl; +// std::cout << m_accRg << std::endl; + +// std::cout << "accelerometer velocity\n" << m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerVelocity(m_name).coeffs() << std::endl; + + m_linVel = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerVelocity(m_name).coeffs().segment(0, 3); + m_angVel = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerVelocity(m_name).coeffs().segment(3, 3); + +// std::cout << "Lin vel" << std::endl; +// std::cout << m_linVel << std::endl; +// std::cout << "Ang vel" << std::endl; +// std::cout << m_angVel << std::endl; + + m_vCrossW = m_linVel.cross(m_angVel); - m_vCrossW = linVel.cross(angVel); +// std::cout << "m_vCrossW" << std::endl; +// std::cout << m_vCrossW << std::endl; m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) = m_JdotNu.segment(0, 3) + m_JvdotBase.segment(0, 3) - m_vCrossW - m_accRg + m_bias; diff --git a/src/Estimators/src/SubModelKinDynWrapper.cpp b/src/Estimators/src/SubModelKinDynWrapper.cpp index a0876de522..defc59c2d4 100644 --- a/src/Estimators/src/SubModelKinDynWrapper.cpp +++ b/src/Estimators/src/SubModelKinDynWrapper.cpp @@ -124,7 +124,7 @@ bool RDE::SubModelKinDynWrapper::initialize(const RDE::SubModel& subModel) bool RDE::SubModelKinDynWrapper::updateState(manif::SE3d::Tangent& robotBaseAcceleration, Eigen::Ref robotJointAcceleration, - bool isCorrectStep) + bool updateRobotDynamicsOnly) { constexpr auto logPrefix = "[BipedalLocomotion::Estimators::SubModelKinDynWrapper::" "updateKinDynState]"; @@ -169,10 +169,10 @@ bool RDE::SubModelKinDynWrapper::updateState(manif::SE3d::Tangent& robotBaseAcce return false; } - return updateDynamicsVariableState(isCorrectStep); + return updateDynamicsVariableState(updateRobotDynamicsOnly); } -bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState(bool isCorrectStep) +bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState(bool updateRobotDynamicsOnly) { constexpr auto logPrefix = "[BipedalLocomotion::Estimators::SubModelKinDynWrapper::" "updateDynamicsVariableState]"; @@ -206,7 +206,7 @@ bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState(bool isCorrectStep) } } - if (isCorrectStep) + if (!updateRobotDynamicsOnly) { // Update accelerometer jacobians, dJnu, rotMatrix for (int idx = 0; idx < m_subModel.getAccelerometerList().size(); idx++) diff --git a/src/Estimators/src/UkfMeasurement.cpp b/src/Estimators/src/UkfMeasurement.cpp index 65ddd720d2..9f4351d4de 100644 --- a/src/Estimators/src/UkfMeasurement.cpp +++ b/src/Estimators/src/UkfMeasurement.cpp @@ -32,10 +32,11 @@ struct RDE::UkfMeasurement::Impl Eigen::Vector3d gravity{0, 0, -Math::StandardAccelerationOfGravitation}; /**< Gravity vector. */ Eigen::MatrixXd covarianceR; /**< Covariance matrix. */ - std::size_t measurementSize; /**< Length of the measurement vector. */ + int measurementSize{0}; /**< Length of the measurement vector. */ double dT; /**< Sampling time */ - std::map> dynamicsList; /**< List of the dynamics composing the process model. */ + std::vector>> dynamicsList; +// std::map> dynamicsList; /**< List of the dynamics composing the process model. */ System::VariablesHandler measurementVariableHandler; /**< Variable handler describing the measurement vector. */ System::VariablesHandler stateVariableHandler; /**< Variable handler describing the state vector. */ @@ -74,6 +75,8 @@ struct RDE::UkfMeasurement::Impl int offsetMeasurement; /**< Offset used to fill the measurement vector. */ + bool updateRobotDynamicsOnly{false}; + void unpackState() { jointVelocityState = currentState.segment(stateVariableHandler.getVariable("ds").offset, @@ -124,13 +127,13 @@ struct RDE::UkfMeasurement::Impl // compute joint acceleration per each sub-model containing the accelerometer for (int subModelIdx = 0; subModelIdx < subModelList.size(); subModelIdx++) { + // Update the kindyn wrapper object of the submodel + kinDynWrapperList[subModelIdx]->updateState(ukfInput.robotBaseAcceleration, + jointAccelerationState, + updateRobotDynamicsOnly); + if (subModelList[subModelIdx].getModel().getNrOfDOFs() > 0) { - // Update the kindyn wrapper object of the submodel - kinDynWrapperList[subModelIdx]->updateState(ukfInput.robotBaseAcceleration, - jointAccelerationState, - false); - totalTorqueFromContacts[subModelIdx].setZero(); // Contribution of FT measurements @@ -222,26 +225,44 @@ bool RDE::UkfMeasurement::finalize(const System::VariablesHandler& handler) m_pimpl->measurementSize = 0; // finalize all the dynamics - for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// { +// if(!dynamics->finalize(handler)) +// { +// BipedalLocomotion::log()->error("{} Error while finalizing the dynamics named {}", logPrefix, name); +// return false; +// } + +// m_pimpl->measurementSize += dynamics->size(); +// } + for (int indexDyn1 = 0; indexDyn1 < m_pimpl->dynamicsList.size(); indexDyn1++) { - if(!dynamics->finalize(handler)) + if(!m_pimpl->dynamicsList[indexDyn1].second->finalize(handler)) { - BipedalLocomotion::log()->error("{} Error while finalizing the dynamics named {}", logPrefix, name); + BipedalLocomotion::log()->error("{} Error while finalizing the dynamics named {}", logPrefix, m_pimpl->dynamicsList[indexDyn1].first); return false; } - m_pimpl->measurementSize += dynamics->size(); + m_pimpl->measurementSize += m_pimpl->dynamicsList[indexDyn1].second->size(); } // Set value of measurement covariance matrix m_pimpl->covarianceR.resize(m_pimpl->measurementSize, m_pimpl->measurementSize); + m_pimpl->covarianceR.setZero(); + +// for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// { +// m_pimpl->measurementVariableHandler.addVariable(name, dynamics->getCovariance().size()); - for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// m_pimpl->covarianceR.block(m_pimpl->measurementVariableHandler.getVariable(name).offset, m_pimpl->measurementVariableHandler.getVariable(name).offset, +// m_pimpl->measurementVariableHandler.getVariable(name).size, m_pimpl->measurementVariableHandler.getVariable(name).size) = dynamics->getCovariance().asDiagonal(); +// } + for (int indexDyn2 = 0; indexDyn2 < m_pimpl->dynamicsList.size(); indexDyn2++) { - m_pimpl->measurementVariableHandler.addVariable(name, dynamics->getCovariance().size()); + m_pimpl->measurementVariableHandler.addVariable(m_pimpl->dynamicsList[indexDyn2].first, m_pimpl->dynamicsList[indexDyn2].second->getCovariance().size()); - m_pimpl->covarianceR.block(m_pimpl->measurementVariableHandler.getVariable(name).offset, m_pimpl->measurementVariableHandler.getVariable(name).offset, - m_pimpl->measurementVariableHandler.getVariable(name).size, m_pimpl->measurementVariableHandler.getVariable(name).size) = dynamics->getCovariance().asDiagonal(); + m_pimpl->covarianceR.block(m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).offset, m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).offset, + m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).size, m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).size) = m_pimpl->dynamicsList[indexDyn2].second->getCovariance().asDiagonal(); } m_pimpl->jointVelocityState.resize(m_pimpl->kinDynFullModel->model().getNrOfDOFs()); @@ -267,6 +288,8 @@ bool RDE::UkfMeasurement::finalize(const System::VariablesHandler& handler) m_pimpl->measurementDescription = bfl::VectorDescription(m_pimpl->measurementSize); + m_pimpl->predictedMeasurement.resize(m_pimpl->measurementSize, 2*m_pimpl->stateVariableHandler.getNumberOfVariables()+1); + m_pimpl->isFinalized = true; return true; @@ -375,7 +398,8 @@ std::unique_ptr RDE::UkfMeasurement::build(std::weak_ptrinitialize(dynamicsGroup); // add dynamics to the list - measurement->m_pimpl->dynamicsList.insert({dynamicsName, dynamicsInstance}); +// measurement->m_pimpl->dynamicsList.insert({dynamicsName, dynamicsInstance}); + measurement->m_pimpl->dynamicsList.emplace_back(dynamicsName, dynamicsInstance); } measurement->m_pimpl->inputDescription = bfl::VectorDescription(stateVariableHandler.getNumberOfVariables()); @@ -417,8 +441,6 @@ std::pair RDE::UkfMeasurement::predictedMeasure(const Eigen::Re { constexpr auto logPrefix = "[UkfMeasurement::propagate]"; - m_pimpl->predictedMeasurement.resize(m_pimpl->measurementSize, cur_states.cols()); - // Check that everything is initialized and set if (!m_pimpl->isFinalized) { @@ -435,9 +457,9 @@ std::pair RDE::UkfMeasurement::predictedMeasure(const Eigen::Re // Get input of ukf from provider m_pimpl->ukfInput = m_pimpl->ukfInputProvider->getOutput(); - for (int index = 0; index < cur_states.cols(); index++) + for (int sample = 0; sample < cur_states.cols(); sample++) { - m_pimpl->currentState = cur_states.block(0, index, cur_states.rows(), 1);; + m_pimpl->currentState = cur_states.block(0, sample, cur_states.rows(), 1);; m_pimpl->unpackState(); @@ -448,8 +470,6 @@ std::pair RDE::UkfMeasurement::predictedMeasure(const Eigen::Re m_pimpl->jointVelocityState, m_pimpl->gravity); - m_pimpl->unpackState(); - if (!m_pimpl->updateState()) { BipedalLocomotion::log()->error("{} The joint accelerations are not updated.", logPrefix); @@ -462,23 +482,39 @@ std::pair RDE::UkfMeasurement::predictedMeasure(const Eigen::Re // This could be parallelized // Update all the dynamics - for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// { +// std::cout << "variable name = " << name << " , variable offset = " << m_pimpl->measurementVariableHandler.getVariable(name).offset << std::endl; + +// dynamics->setState(m_pimpl->currentState); + +// dynamics->setInput(m_pimpl->ukfInput); + +// if (!dynamics->update()) +// { +// BipedalLocomotion::log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, name); +// throw std::runtime_error("Error"); +// } + +// m_pimpl->tempPredictedMeas.segment(m_pimpl->measurementVariableHandler.getVariable(name).offset, +// m_pimpl->measurementVariableHandler.getVariable(name).size) = dynamics->getUpdatedVariable(); +// } + for (int indexDyn = 0; indexDyn < m_pimpl->dynamicsList.size(); indexDyn++) { - dynamics->setState(m_pimpl->currentState); + m_pimpl->dynamicsList[indexDyn].second->setState(m_pimpl->currentState); - dynamics->setInput(m_pimpl->ukfInput); + m_pimpl->dynamicsList[indexDyn].second->setInput(m_pimpl->ukfInput); - if (!dynamics->update()) + if (!m_pimpl->dynamicsList[indexDyn].second->update()) { - BipedalLocomotion::log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, name); + BipedalLocomotion::log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, m_pimpl->dynamicsList[indexDyn].first); throw std::runtime_error("Error"); } - m_pimpl->tempPredictedMeas.segment(m_pimpl->measurementVariableHandler.getVariable(name).offset, - m_pimpl->measurementVariableHandler.getVariable(name).size) = dynamics->getUpdatedVariable(); + m_pimpl->tempPredictedMeas.segment(m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn].first).offset, + m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn].first).size) = m_pimpl->dynamicsList[indexDyn].second->getUpdatedVariable(); } - m_pimpl->predictedMeasurement.block(0, index, m_pimpl->measurementSize, 1) = m_pimpl->tempPredictedMeas; - + m_pimpl->predictedMeasurement.block(0, sample, m_pimpl->measurementSize, 1) = m_pimpl->tempPredictedMeas; } return std::make_pair(true, m_pimpl->predictedMeasurement); @@ -517,25 +553,46 @@ bool RDE::UkfMeasurement::freeze(const bfl::Data& data) m_pimpl->measurementMap = bfl::any::any_cast>(data); - for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// { +// m_pimpl->offsetMeasurement = m_pimpl->measurementVariableHandler.getVariable(name).offset; + +// if(m_pimpl->measurementMap.count(name) == 0) +// { +// BipedalLocomotion::log()->error("{} Measurement with name `{}` not found.", logPrefix, name); +// return false; +// } + +// // If more sub-models share the same accelerometer or gyroscope sensor, the measurement vector is concatenated +// // a number of times equal to the number of sub-models using the sensor. +// while(m_pimpl->offsetMeasurement < +// (m_pimpl->measurementVariableHandler.getVariable(name).offset + m_pimpl->measurementVariableHandler.getVariable(name).size)) +// { +// m_pimpl->measurement.segment(m_pimpl->offsetMeasurement, m_pimpl->measurementMap[name].size()) +// = m_pimpl->measurementMap[name]; + +// m_pimpl->offsetMeasurement += m_pimpl->measurementMap[name].size(); +// } +// } + for (int index = 0; index < m_pimpl->dynamicsList.size(); index++) { - m_pimpl->offsetMeasurement = m_pimpl->measurementVariableHandler.getVariable(name).offset; + m_pimpl->offsetMeasurement = m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[index].first).offset; - if(m_pimpl->measurementMap.count(name) == 0) + if(m_pimpl->measurementMap.count(m_pimpl->dynamicsList[index].first) == 0) { - BipedalLocomotion::log()->error("{} Measurement with name `{}` not found.", logPrefix, name); + BipedalLocomotion::log()->error("{} Measurement with name `{}` not found.", logPrefix, m_pimpl->dynamicsList[index].first); return false; } // If more sub-models share the same accelerometer or gyroscope sensor, the measurement vector is concatenated // a number of times equal to the number of sub-models using the sensor. while(m_pimpl->offsetMeasurement < - (m_pimpl->measurementVariableHandler.getVariable(name).offset + m_pimpl->measurementVariableHandler.getVariable(name).size)) + (m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[index].first).offset + m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[index].first).size)) { - m_pimpl->measurement.segment(m_pimpl->offsetMeasurement, m_pimpl->measurementMap[name].size()) - = m_pimpl->measurementMap[name]; + m_pimpl->measurement.segment(m_pimpl->offsetMeasurement, m_pimpl->measurementMap[m_pimpl->dynamicsList[index].first].size()) + = m_pimpl->measurementMap[m_pimpl->dynamicsList[index].first]; - m_pimpl->offsetMeasurement += m_pimpl->measurementMap[name].size(); + m_pimpl->offsetMeasurement += m_pimpl->measurementMap[m_pimpl->dynamicsList[index].first].size(); } } diff --git a/src/Estimators/src/UkfState.cpp b/src/Estimators/src/UkfState.cpp index d2a08e1ee3..b0f0ca6991 100644 --- a/src/Estimators/src/UkfState.cpp +++ b/src/Estimators/src/UkfState.cpp @@ -34,7 +34,7 @@ struct RDE::UkfState::Impl std::size_t stateSize; /**< Length of the state vector. */ double dT; /**< Sampling time */ - std::map> dynamicsList; /**< List of the dynamics composing the process model. */ + std::vector>> dynamicsList; /**< List of the dynamics composing the process model. */ System::VariablesHandler stateVariableHandler; /**< Variable handler describing the state vector. */ @@ -63,6 +63,8 @@ struct RDE::UkfState::Impl std::vector torqueFromContact; /**< Joint torques due to a specific contact. */ Math::Wrenchd wrench; /**< Joint torques due to a specific contact. */ + bool updateRobotDynamicsOnly{true}; + void unpackState() { jointVelocityState = currentState.segment(stateVariableHandler.getVariable("ds").offset, @@ -110,16 +112,16 @@ struct RDE::UkfState::Impl jointVelocityState, gravity); - // compute joint acceleration per each sub-model containing the accelerometer + // compute joint acceleration per each sub-model for (int subModelIdx = 0; subModelIdx < subModelList.size(); subModelIdx++) { + // Update the kindyn wrapper object of the submodel + kinDynWrapperList[subModelIdx]->updateState(ukfInput.robotBaseAcceleration, + jointAccelerationState, + updateRobotDynamicsOnly); + if (subModelList[subModelIdx].getModel().getNrOfDOFs() > 0) { - // Update the kindyn wrapper object of the submodel - kinDynWrapperList[subModelIdx]->updateState(ukfInput.robotBaseAcceleration, - jointAccelerationState, - false); - totalTorqueFromContacts[subModelIdx].setZero(); // Contribution of FT measurements @@ -211,15 +213,25 @@ bool RDE::UkfState::finalize(const System::VariablesHandler& handler) m_pimpl->stateSize = 0; // finalize all the dynamics - for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// { +// if(!dynamics->finalize(handler)) +// { +// log()->error("{} Error while finalizing the dynamics named {}", logPrefix, name); +// return false; +// } + +// m_pimpl->stateSize += dynamics->size(); +// } + for (int indexDyn1 = 0; indexDyn1 < m_pimpl->dynamicsList.size(); indexDyn1++) { - if(!dynamics->finalize(handler)) + if(!m_pimpl->dynamicsList[indexDyn1].second->finalize(handler)) { - log()->error("{} Error while finalizing the dynamics named {}", logPrefix, name); + log()->error("{} Error while finalizing the dynamics named {}", logPrefix, m_pimpl->dynamicsList[indexDyn1].first); return false; } - m_pimpl->stateSize += dynamics->size(); + m_pimpl->stateSize += m_pimpl->dynamicsList[indexDyn1].second->size(); } @@ -230,13 +242,22 @@ bool RDE::UkfState::finalize(const System::VariablesHandler& handler) m_pimpl->initialCovariance.resize(m_pimpl->stateSize, m_pimpl->stateSize); m_pimpl->initialCovariance.setZero(); - for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// { +// m_pimpl->covarianceQ.block(handler.getVariable(name).offset, handler.getVariable(name).offset, +// handler.getVariable(name).size, handler.getVariable(name).size) = dynamics->getCovariance().asDiagonal(); + +// m_pimpl->initialCovariance.block(handler.getVariable(name).offset, handler.getVariable(name).offset, +// handler.getVariable(name).size, handler.getVariable(name).size) = dynamics->getInitialStateCovariance().asDiagonal(); + +// } + for (int indexDyn2 = 0; indexDyn2 < m_pimpl->dynamicsList.size(); indexDyn2++) { - m_pimpl->covarianceQ.block(handler.getVariable(name).offset, handler.getVariable(name).offset, - handler.getVariable(name).size, handler.getVariable(name).size) = dynamics->getCovariance().asDiagonal(); + m_pimpl->covarianceQ.block(handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).offset, handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).offset, + handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).size, handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).size) = m_pimpl->dynamicsList[indexDyn2].second->getCovariance().asDiagonal(); - m_pimpl->initialCovariance.block(handler.getVariable(name).offset, handler.getVariable(name).offset, - handler.getVariable(name).size, handler.getVariable(name).size) = dynamics->getInitialStateCovariance().asDiagonal(); + m_pimpl->initialCovariance.block(handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).offset, handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).offset, + handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).size, handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).size) = m_pimpl->dynamicsList[indexDyn2].second->getInitialStateCovariance().asDiagonal(); } @@ -370,7 +391,8 @@ std::unique_ptr RDE::UkfState::build(std::weak_ptrinitialize(dynamicsGroup); // add dynamics to the list - state->m_pimpl->dynamicsList.insert({dynamicsName, dynamicsInstance}); + state->m_pimpl->dynamicsList.emplace_back(dynamicsName, dynamicsInstance); +// state->m_pimpl->dynamicsList.insert({dynamicsName, dynamicsInstance}); } // finalize estimator @@ -423,9 +445,9 @@ void RDE::UkfState::propagate(const Eigen::Ref& cur_state prop_states.resize(cur_states.rows(), cur_states.cols()); - for (int index = 0; index < cur_states.cols(); index++) + for (int sample = 0; sample < cur_states.cols(); sample++) { - m_pimpl->currentState = cur_states.block(0, index, cur_states.rows(), 1); + m_pimpl->currentState = cur_states.block(0, sample, cur_states.rows(), 1); m_pimpl->unpackState(); @@ -441,23 +463,39 @@ void RDE::UkfState::propagate(const Eigen::Ref& cur_state // This could be parallelized // Update all the dynamics - for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// { +// dynamics->setState(m_pimpl->currentState); + +// dynamics->setInput(m_pimpl->ukfInput); + +// if (!dynamics->update()) +// { +// log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, name); +// throw std::runtime_error("Error"); +// } + +// m_pimpl->nextState.segment(m_pimpl->stateVariableHandler.getVariable(name).offset, +// m_pimpl->stateVariableHandler.getVariable(name).size) = dynamics->getUpdatedVariable(); +// } + + for (int indexDyn = 0; indexDyn < m_pimpl->dynamicsList.size(); indexDyn++) { - dynamics->setState(m_pimpl->currentState); + m_pimpl->dynamicsList[indexDyn].second->setState(m_pimpl->currentState); - dynamics->setInput(m_pimpl->ukfInput); + m_pimpl->dynamicsList[indexDyn].second->setInput(m_pimpl->ukfInput); - if (!dynamics->update()) + if (!m_pimpl->dynamicsList[indexDyn].second->update()) { - log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, name); + log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, m_pimpl->dynamicsList[indexDyn].first); throw std::runtime_error("Error"); } - m_pimpl->nextState.segment(m_pimpl->stateVariableHandler.getVariable(name).offset, - m_pimpl->stateVariableHandler.getVariable(name).size) = dynamics->getUpdatedVariable(); + m_pimpl->nextState.segment(m_pimpl->stateVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn].first).offset, + m_pimpl->stateVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn].first).size) = m_pimpl->dynamicsList[indexDyn].second->getUpdatedVariable(); } - prop_states.block(0, index, cur_states.rows(), 1) = m_pimpl->nextState; + prop_states.block(0, sample, cur_states.rows(), 1) = m_pimpl->nextState; } } diff --git a/src/Estimators/src/ZeroVelocityDynamics.cpp b/src/Estimators/src/ZeroVelocityDynamics.cpp index 34c674b302..ca87553cdd 100644 --- a/src/Estimators/src/ZeroVelocityDynamics.cpp +++ b/src/Estimators/src/ZeroVelocityDynamics.cpp @@ -62,7 +62,6 @@ bool RDE::ZeroVelocityDynamics::initialize(std::weak_ptr(); std::vector accNameList = {"r_leg_ft_acc"}; - std::vector accFrameList = {"r_leg_ft_sensor"}; + std::vector accFrameList = {"r_leg_ft"}; accGroup->setParameter("names", accNameList); accGroup->setParameter("frames", accFrameList); REQUIRE(modelParamHandler->setGroup("ACCELEROMETER", accGroup)); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp index f1a17b677b..beb57a0fa9 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp @@ -121,14 +121,19 @@ void createInitialState(std::shared_ptr kinDynFull // Set values initialState.ds.setZero(); - initialState.tau_m << -1.24764e+01, 1.03400e-01, -4.70000e-03, -3.16350e+00, 4.21800e-01, 4.85000e-01; +// initialState.tau_m << -1.24764e+01, 1.03400e-01, -4.70000e-03, -3.16350e+00, 4.21800e-01, 4.85000e-01; + initialState.tau_m << 22.644 , 13.5454, -7.6093, 19.3029, -15.0072, -0.805; initialState.tau_F.setZero(); - initialState.ftWrenches["r_leg_ft"] << -3.83709731e+01, -8.45653659e+00, 9.25208925e+01, 3.12676978e+00, -9.79714657e+00, 4.01285264e-01; - initialState.ftWrenches["r_foot_front_ft"] << 5.22305136e-01, -4.88067107e-01, 1.61011089e+00, -9.13330381e-03, -8.39265034e-03, 4.18725767e-04; - initialState.ftWrenches["r_foot_rear_ft"] << 5.19688377e-01, -4.85621881e-01, 1.60204420e+00, -8.32204636e-03, -9.16952530e-03, -7.99299795e-05; - initialState.ftWrenchesBiases["r_leg_ft_bias"] << -1.89410386e+01, 7.33674253e+01, -6.04774355e+01, 1.34890092e-02, -3.02023625e+00, 7.01925185e-01; - initialState.ftWrenchesBiases["r_foot_front_ft_bias"] << -4.85874907e+01, -3.90627141e+01, -3.89636265e+01, -1.83127438e-01, 9.51385814e-01, 2.97127661e-01; - initialState.ftWrenchesBiases["r_foot_rear_ft_bias"] << -3.75985458e+01, -6.87282453e+01, -1.41893873e+00, -2.24845286e+00, 1.18104453e+00, 3.75446141e-01; +// initialState.ftWrenches["r_leg_ft"] << -3.83709731e+01, -8.45653659e+00, 9.25208925e+01, 3.12676978e+00, -9.79714657e+00, 4.01285264e-01; +// initialState.ftWrenches["r_foot_front_ft"] << 5.22305136e-01, -4.88067107e-01, 1.61011089e+00, -9.13330381e-03, -8.39265034e-03, 4.18725767e-04; +// initialState.ftWrenches["r_foot_rear_ft"] << 5.19688377e-01, -4.85621881e-01, 1.60204420e+00, -8.32204636e-03, -9.16952530e-03, -7.99299795e-05; +// initialState.ftWrenchesBiases["r_leg_ft_bias"] << -1.89410386e+01, 7.33674253e+01, -6.04774355e+01, 1.34890092e-02, -3.02023625e+00, 7.01925185e-01; +// initialState.ftWrenchesBiases["r_foot_front_ft_bias"] << -4.85874907e+01, -3.90627141e+01, -3.89636265e+01, -1.83127438e-01, 9.51385814e-01, 2.97127661e-01; +// initialState.ftWrenchesBiases["r_foot_rear_ft_bias"] << -3.75985458e+01, -6.87282453e+01, -1.41893873e+00, -2.24845286e+00, 1.18104453e+00, 3.75446141e-01; + + initialState.ftWrenches["r_leg_ft"] << 49.45480758, 73.91349929, 46.85057916, -15.38562865, 9.04317083, 1.97395434; + initialState.ftWrenches["r_foot_front_ft"] << 1.38697938e-01, 8.71035288e-01, 1.52496873e+00, 1.36409975e-02, -1.99708849e-03, -9.99651158e-05; + initialState.ftWrenches["r_foot_rear_ft"] << 1.38003059e-01, 8.66671384e-01, 1.51732861e+00, 1.70397864e-02, -2.03056058e-03, -3.89970831e-04; } void createInput(std::shared_ptr kinDynFullModel, @@ -152,7 +157,8 @@ void createInput(std::shared_ptr kinDynFullModel, input.baseAcceleration = baseAcc; input.jointPositions.resize(kinDynFullModel->model().getNrOfDOFs()); - input.jointPositions << -3.81866275e-01, 1.27512464e-01, 3.83496133e-04, -2.67488553e-02, -9.77915140e-03, 9.58740333e-05; +// input.jointPositions << -3.81866275e-01, 1.27512464e-01, 3.83496133e-04, -2.67488553e-02, -9.77915140e-03, 9.58740333e-05; + input.jointPositions << 0.91693925, 0.69777121, 0.00249272, -0.68166438, -0.07698685, -0.08628663; input.jointVelocities.resize(kinDynFullModel->model().getNrOfDOFs()); input.jointVelocities.setZero(); @@ -166,16 +172,20 @@ void createInput(std::shared_ptr kinDynFullModel, // input.motorCurrents = jointTorques.array() / (gearRatio.array() * torqueConstant.array()); input.motorCurrents.resize(kinDynFullModel->model().getNrOfDOFs()); - input.motorCurrents << -1.124e+00, -2.200e-02, -1.000e-03, -2.850e-01, 3.800e-02, 1.940e-01; +// input.motorCurrents << -1.124e+00, -2.200e-02, -1.000e-03, -2.850e-01, 3.800e-02, 1.940e-01; + input.motorCurrents << 2.04 , -2.882, -1.619, 1.739, -1.352, -0.322; input.ftWrenches["r_leg_ft"] = Eigen::VectorXd(6).setZero(); - input.ftWrenches["r_leg_ft"] << -57.31201172, 64.91088867, 32.04345703, 3.14025879, -12.81738281, 1.10321045; +// input.ftWrenches["r_leg_ft"] << -57.31201172, 64.91088867, 32.04345703, 3.14025879, -12.81738281, 1.10321045; + input.ftWrenches["r_leg_ft"] << 49.45480758, 73.91349929, 46.85057916, -15.38562865, 9.04317083, 1.97395434; input.ftWrenches["r_foot_front_ft"] = Eigen::VectorXd(6).setZero(); - input.ftWrenches["r_foot_front_ft"] << -48.06518555, -39.55078125, -37.35351562, -0.19226074, 0.94299316, 0.29754639; +// input.ftWrenches["r_foot_front_ft"] << -48.06518555, -39.55078125, -37.35351562, -0.19226074, 0.94299316, 0.29754639; + input.ftWrenches["r_foot_front_ft"] << 1.38697938e-01, 8.71035288e-01, 1.52496873e+00, 1.36409975e-02, -1.99708849e-03, -9.99651158e-05; input.ftWrenches["r_foot_rear_ft"] = Eigen::VectorXd(6).setZero(); - input.ftWrenches["r_foot_rear_ft"] << -37.07885742, -69.21386719, 0.18310547, -2.2567749, 1.171875, 0.37536621; +// input.ftWrenches["r_foot_rear_ft"] << -37.07885742, -69.21386719, 0.18310547, -2.2567749, 1.171875, 0.37536621; + input.ftWrenches["r_foot_rear_ft"] << 1.38003059e-01, 8.66671384e-01, 1.51732861e+00, 1.70397864e-02, -2.03056058e-03, -3.89970831e-04; std::vector accList; auto accGroup = modelHandler.lock()->getGroup("ACCELEROMETER").lock(); @@ -185,9 +195,12 @@ void createInput(std::shared_ptr kinDynFullModel, input.linearAccelerations[acc] = Eigen::VectorXd(3).setZero(); input.linearAccelerations[acc](2) = - BipedalLocomotion::Math::StandardAccelerationOfGravitation; } - input.linearAccelerations["r_leg_ft_acc"] << 3.72, 0.8, -9.33; - input.linearAccelerations["r_foot_front_ft_acc"] << 4.01, -2.38, 8.74; - input.linearAccelerations["r_foot_rear_ft_acc"] << 3.77, -2.39, 8.94; +// input.linearAccelerations["r_leg_ft_acc"] << 3.72, 0.8, -9.33; +// input.linearAccelerations["r_foot_front_ft_acc"] << 4.01, -2.38, 8.74; +// input.linearAccelerations["r_foot_rear_ft_acc"] << 3.77, -2.39, 8.94; + input.linearAccelerations["r_leg_ft_acc"] << -3.76, -7.65, -4.32; + input.linearAccelerations["r_foot_front_ft_acc"] << 1.44, 5.08, 8.11; + input.linearAccelerations["r_foot_rear_ft_acc"] << 2.12, 5.23, 8.13; std::vector gyroList; auto gyroGroup = modelHandler.lock()->getGroup("GYROSCOPE").lock(); @@ -196,9 +209,12 @@ void createInput(std::shared_ptr kinDynFullModel, { input.angularVelocities[gyro] = Eigen::VectorXd(3).setZero(); } - input.angularVelocities["r_leg_ft_gyro"] << 0.03708825, -0.06654068, -0.00109083; - input.angularVelocities["r_foot_front_ft_gyro"] << 0.0567232 , 0.04799655, -0.01308997; - input.angularVelocities["r_foot_rear_ft_gyro"] << 0.06108652, 0.04690572, -0.01090831; +// input.angularVelocities["r_leg_ft_gyro"] << 0.03708825, -0.06654068, -0.00109083; +// input.angularVelocities["r_foot_front_ft_gyro"] << 0.0567232 , 0.04799655, -0.01308997; +// input.angularVelocities["r_foot_rear_ft_gyro"] << 0.06108652, 0.04690572, -0.01090831; + input.angularVelocities["r_leg_ft_gyro"] << -0.00654498, 0.0, 0.00109083; + input.angularVelocities["r_foot_front_ft_gyro"] << -0.00327249, 0.0, 0.00218166; + input.angularVelocities["r_foot_rear_ft_gyro"] << -0.00545415, 0.00109083, 0.00218166; } TEST_CASE("RobotDynamicsEstimator") @@ -278,12 +294,16 @@ TEST_CASE("RobotDynamicsEstimator") subModelList, measurement); - auto tic = BipedalLocomotion::clock().now(); - REQUIRE(estimator->setInput(measurement)); - REQUIRE(estimator->advance()); - auto toc = BipedalLocomotion::clock().now(); +// for (int i = 0; i<700; i++) +// { +// std::cout << "index = " << i << std::endl; + auto tic = BipedalLocomotion::clock().now(); + REQUIRE(estimator->setInput(measurement)); + REQUIRE(estimator->advance()); + auto toc = BipedalLocomotion::clock().now(); - BipedalLocomotion::log()->error("{}", toc - tic); + BipedalLocomotion::log()->error("{}", toc - tic); - RobotDynamicsEstimatorOutput result = estimator->getOutput(); + RobotDynamicsEstimatorOutput result = estimator->getOutput(); +// } } diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/model.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/model.ini index 3b0061a9ea..66a320395c 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/config/model.ini +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/model.ini @@ -10,8 +10,8 @@ associated_joints ("r_leg_ft_sensor", "r_foot_front_ft_sensor", "r_foot_rear_ft_ [ACCELEROMETER] names ("r_leg_ft_acc", "r_foot_front_ft_acc", "r_foot_rear_ft_acc") -frames ("r_leg_ft_sensor", "r_foot_front_ft_sensor", "r_foot_rear_ft_sensor") +frames ("r_leg_ft", "r_foot_front_ft", "r_foot_rear_ft") [GYROSCOPE] names ("r_leg_ft_gyro", "r_foot_front_ft_gyro", "r_foot_rear_ft_gyro") -frames ("r_leg_ft_sensor", "r_foot_front_ft_sensor", "r_foot_rear_ft_sensor") +frames ("r_leg_ft", "r_foot_front_ft", "r_foot_rear_ft") diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini index 8653ef21f0..a708005f91 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini @@ -3,6 +3,8 @@ dynamics_list ("JOINT_VELOCITY", "MOTOR_CURRENT", "RIGHT_LEG_ACC", "RIGHT_FOOT_FRONT_ACC", "RIGHT_FOOT_REAR_ACC", "RIGHT_LEG_GYRO", "RIGHT_FOOT_FRONT_GYRO", "RIGHT_FOOT_REAR_GYRO") +# Available dynamics = ["ZeroVelocityDynamics", "AccelerometerMeasurementDynamics", "GyroscopeMeasurementDynamics", "MotorCurrentMeasurementDynamics"] + [JOINT_VELOCITY] name "ds" elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") @@ -46,14 +48,14 @@ use_bias 1 dynamic_model "AccelerometerMeasurementDynamics" [RIGHT_FOOT_FRONT_ACC] -name "r_leg_ft_acc" +name "r_foot_front_ft_acc" elements ("x", "y", "z") covariance (2.3e-3, 1.9e-3, 3.1e-3) use_bias 1 dynamic_model "AccelerometerMeasurementDynamics" [RIGHT_FOOT_REAR_ACC] -name "r_leg_ft_acc" +name "r_foot_rear_ft_acc" elements ("x", "y", "z") covariance (2.5e-3, 2.3e-3, 3e-3) use_bias 1 @@ -67,14 +69,14 @@ use_bias 1 dynamic_model "GyroscopeMeasurementDynamics" [RIGHT_FOOT_FRONT_GYRO] -name "r_leg_ft_gyro" +name "r_foot_front_ft_gyro" elements ("x", "y", "z") covariance (4.1e-4, 3.3e-4, 3.2e-4) use_bias 1 dynamic_model "GyroscopeMeasurementDynamics" [RIGHT_FOOT_REAR_GYRO] -name "r_leg_ft_gyro" +name "r_foot_rear_ft_gyro" elements ("x", "y", "z") covariance (4.9e-4, 4.2e-4, 3.3e-4) use_bias 1 diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini index 8ce1901184..d182e02031 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini @@ -1,7 +1,9 @@ dynamics_list ("JOINT_VELOCITY", "MOTOR_TORQUE", "FRICTION_TORQUE", "RIGHT_LEG_FT", "RIGHT_FOOT_FRONT_FT", "RIGHT_FOOT_REAR_FT", - "RIGHT_LEG_FT_BIAS", "RIGHT_FOOT_FRONT_FT_BIAS", "RIGHT_FOOT_REAR_FT_BIAS", "RIGHT_LEG_ACC_BIAS", - "RIGHT_FOOT_FRONT_ACC_BIAS", "RIGHT_FOOT_REAR_ACC_BIAS", "RIGHT_LEG_GYRO_BIAS", "RIGHT_FOOT_FRONT_GYRO_BIAS", - "RIGHT_FOOT_REAR_GYRO_BIAS") + "RIGHT_LEG_FT_BIAS", "RIGHT_FOOT_FRONT_FT_BIAS", "RIGHT_FOOT_REAR_FT_BIAS", + "RIGHT_LEG_ACC_BIAS", "RIGHT_FOOT_FRONT_ACC_BIAS", "RIGHT_FOOT_REAR_ACC_BIAS", + "RIGHT_LEG_GYRO_BIAS", "RIGHT_FOOT_FRONT_GYRO_BIAS", "RIGHT_FOOT_REAR_GYRO_BIAS") + +# Available dynamics = ["ZeroVelocityDynamics", "JointVelocityStateDynamics", "FrictionTorqueStateDynamics"] [JOINT_VELOCITY] name "ds"