From ced9560ea7bd7d97dcfc9f828d7af954a7a30958 Mon Sep 17 00:00:00 2001 From: gianlucamilani Date: Tue, 18 Jun 2024 13:26:16 +0200 Subject: [PATCH] added comments --- src/ID/src/InverseDynamics.cpp | 147 +++++++++++++++++++++++---------- 1 file changed, 104 insertions(+), 43 deletions(-) diff --git a/src/ID/src/InverseDynamics.cpp b/src/ID/src/InverseDynamics.cpp index d805be2..b217462 100644 --- a/src/ID/src/InverseDynamics.cpp +++ b/src/ID/src/InverseDynamics.cpp @@ -8,31 +8,39 @@ bool HumanID::initialize(std::weak_ptr kinDyn) { - constexpr auto logPrefix = "[HumanID::intizialize]"; + // Log prefix for this class + constexpr auto logPrefix = "[HumanID::initialize]"; + + // Get the parameters handler pointer auto ptr = handler.lock(); + // Retrieve the 'humanMass' parameter and check if it's obtained successfully if (!ptr->getParameter("humanMass", m_humanMass)) { BiomechanicalAnalysis::log()->error("{} Error getting the 'humanMass' parameter.", logPrefix); return false; } - // Check the validity of the kinDyn object + // Check the validity of the passed kinDyn object if ((kinDyn == nullptr) || (!kinDyn->isValid())) { BiomechanicalAnalysis::log()->error("{} Invalid kinDyn object.", logPrefix); return false; } + // Assign the passed kinDyn object m_kinDyn = kinDyn; - // check if the model path is provided and load the model otherwise use the kinDyn object passed + // Check if the model path is provided and load the model if present, otherwise use the kinDyn object iDynTree::ModelLoader loader; if (ptr->getParameter("modelPath", m_modelPath)) { std::vector jointsList; + + // Retrieve the joints list if available if (ptr->getParameter("jointsList", jointsList)) { + // Load the reduced model from file with the specified joints list if (!loader.loadReducedModelFromFile(m_modelPath, jointsList)) { BiomechanicalAnalysis::log()->error("{} Error loading the model from file {}.", logPrefix, m_modelPath); @@ -40,10 +48,10 @@ bool HumanID::initialize(std::weak_ptrwarn("{} Parameter 'jointsList' not found in the " - "configuration file, loading the model with all the " - "joints.", - logPrefix); + // Warning if 'jointsList' parameter is not found in the configuration file, load the model with all joints + BiomechanicalAnalysis::log()->warn("{} Parameter 'jointsList' not found in the configuration file, loading the model with all the joints.", logPrefix); + + // Load the full model from file if (!loader.loadModelFromFile(m_modelPath)) { BiomechanicalAnalysis::log()->error("{} Error loading the model from file {}.", logPrefix, m_modelPath); @@ -51,19 +59,23 @@ bool HumanID::initialize(std::weak_ptr(); if (!m_kinDynFullModel->loadRobotModel(loader.model())) { BiomechanicalAnalysis::log()->error("{} Error loading the model from file {}.", logPrefix, m_modelPath); return false; } + + // Set the floating base for m_kinDynFullModel m_kinDynFullModel->setFloatingBase(m_kinDyn->getFloatingBase()); m_useFullModel = true; } else { - BiomechanicalAnalysis::log()->warn("{} Error getting the modelPath parameter, using the " - "default model.", - logPrefix); + // Warning if 'modelPath' parameter couldn't be retrieved, use the default model + BiomechanicalAnalysis::log()->warn("{} Error getting the modelPath parameter, using the default model.", logPrefix); + + // Set m_useFullModel to false and m_kinDynFullModel to m_kinDyn m_useFullModel = false; m_kinDynFullModel = m_kinDyn; } @@ -77,7 +89,7 @@ bool HumanID::initialize(std::weak_ptrmodel().getNrOfDOFs()); - // Get the group handlers for initializing the MAPHelper m_jointTorquesHelper object + // Get the group handler for 'JOINT_TORQUES' to initialize the MAPHelper m_jointTorquesHelper object auto jointTorquesHandler = ptr->getGroup("JOINT_TORQUES").lock(); if (jointTorquesHandler == nullptr) { @@ -149,43 +161,62 @@ bool HumanID::updateExtWrenchesMeasurements(const std::unordered_mapgetWorldTransform(m_wrenchSources[i].outputFrame).getRotation()); - } else if (m_wrenchSources[i].type == WrenchSourceType::Fixed) + } + // If the wrench source type is Fixed, compute the wrench and update the measurement vector + else if (m_wrenchSources[i].type == WrenchSourceType::Fixed) { + // Check if the wrench exists in the provided wrenches map if (wrenches.find(m_wrenchSources[i].outputFrame) == wrenches.end()) { BiomechanicalAnalysis::log()->error("{} Wrench {} not found.", logPrefix, m_wrenchSources[i].outputFrame); return false; } + + // Compute the wrench in the output frame's transformed coordinate system m_wrenchSources[i].wrench = m_wrenchSources[i].outputFrameTransform * wrenches.at(m_wrenchSources[i].outputFrame); + // Determine the range of indices in the measurement vector for this sensor iDynTree::IndexRange sensorRange = m_extWrenchesEstimator.berdyHelper.getRangeLinkSensorVariable(iDynTree::BerdySensorTypes::NET_EXT_WRENCH_SENSOR, m_kinDynFullModel->getFrameIndex( m_wrenchSources[i].outputFrame)); + + // Update the measurement vector with the computed wrench components for (int j = 0; j < 6; j++) { m_extWrenchesEstimator.measurement(sensorRange.offset + j) = m_wrenchSources[i].wrench(j); } } } + + // Compute the reaction wrench at the Centroidal Moment Pivot (RCM) in the base frame iDynTree::SpatialForceVector rcmWrench = computeRCMInBaseFrame(); + + // Determine the range of indices in the measurement vector for the RCM sensor iDynTree::IndexRange rcmSensorRange = m_extWrenchesEstimator.berdyHelper.getRangeRCMSensorVariable(iDynTree::BerdySensorTypes::RCM_SENSOR); + + // Update the measurement vector with the computed RCM wrench components for (int i = 0; i < 6; i++) { m_extWrenchesEstimator.measurement(rcmSensorRange.offset + i) = rcmWrench(i); } - return true; + return true; // Return true indicating successful update of the measurement vector } + bool HumanID::solve() { constexpr auto logPrefix = "[HumanID::solve]"; @@ -479,6 +510,7 @@ bool HumanID::initializeExtWrenchesHelper(const std::shared_ptr wrenchSource; if (!groupHandler->getParameter("wrenchSources", wrenchSource)) { @@ -486,6 +518,7 @@ bool HumanID::initializeExtWrenchesHelper(const std::shared_ptrerror("{} Error getting the wrench group {}.", logPrefix, wrench); return false; } + + // Get output frame name if (!wrenchHandler->getParameter("outputFrame", data.outputFrame)) { BiomechanicalAnalysis::log()->error("{} Error getting the name parameter.", logPrefix); return false; } + + // Determine type of wrench source (fixed or dummy) std::string type; if (!wrenchHandler->getParameter("type", type)) { @@ -510,7 +547,7 @@ bool HumanID::initializeExtWrenchesHelper(const std::shared_ptr position, orientation; if (!wrenchHandler->getParameter("position", position)) { @@ -550,6 +589,7 @@ bool HumanID::initializeExtWrenchesHelper(const std::shared_ptr values; if (!wrenchHandler->getParameter("values", values)) { @@ -561,10 +601,15 @@ bool HumanID::initializeExtWrenchesHelper(const std::shared_ptrgetParameter("mu_dyn_variables", m_extWrenchesEstimator.params.priorDynamicsRegularizationExpected)) { BiomechanicalAnalysis::log()->error("{} Error getting the 'mu_dyn_variables' parameter.", logPrefix); @@ -577,6 +622,7 @@ bool HumanID::initializeExtWrenchesHelper(const std::shared_ptr specificElements; if (!groupHandler->getParameter("specificElements", specificElements)) { @@ -584,6 +630,7 @@ bool HumanID::initializeExtWrenchesHelper(const std::shared_ptr covariance; @@ -595,6 +642,7 @@ bool HumanID::initializeExtWrenchesHelper(const std::shared_ptrgetParameter("cov_measurements_RCM_SENSOR", m_extWrenchesEstimator.params.specificMeasurementsCovariance["RCM_" "SENSOR"])) @@ -605,6 +653,7 @@ bool HumanID::initializeExtWrenchesHelper(const std::shared_ptrgetParameter("default_cov_measurements", m_extWrenchesEstimator.params.measurementDefaultCovariance)) { BiomechanicalAnalysis::log()->error("{} Error getting the 'default_cov_measurements' " @@ -613,6 +662,7 @@ bool HumanID::initializeExtWrenchesHelper(const std::shared_ptrerror("{} Error in the consistency of the BerdyOptions " @@ -629,14 +680,15 @@ bool HumanID::initializeExtWrenchesHelper(const std::shared_ptrgetRobotModel(), berdyOptionsExtWrenches)) { BiomechanicalAnalysis::log()->error("{} Error initializing the BerdyHelper object.", logPrefix); return false; } + // Initialize BerdySparseMAPSolver for external wrenches m_extWrenchesEstimator.berdySolver = std::make_unique(m_extWrenchesEstimator.berdyHelper); - if (!m_extWrenchesEstimator.berdySolver->initialize()) { BiomechanicalAnalysis::log()->error("{} Error initializing the BerdySparseMAPSolver " @@ -645,45 +697,49 @@ bool HumanID::initializeExtWrenchesHelper(const std::shared_ptrsecond[i]); - } + wrenchCovariance.setVal(i, m_extWrenchesEstimator.params.measurementDefaultCovariance); - for (std::size_t i = 0; i < 6; i++) - measurementsCovarianceMatrixTriplets.setTriplet( - {berdySensor.range.offset + i, berdySensor.range.offset + i, wrenchCovariance[i]}); - } + // Set specific covariance if configured + auto specificMeasurementsPtr = m_extWrenchesEstimator.params.specificMeasurementsCovariance.find(berdySensor.id); + if (specificMeasurementsPtr != m_extWrenchesEstimator.params.specificMeasurementsCovariance.end()) + { + for (int i = 0; i < 6; i++) + wrenchCovariance.setVal(i, specificMeasurementsPtr->second[i]); + } + + // Store triplet for the measurements covariance matrix + for (std::size_t i = 0; i < 6; i++) + measurementsCovarianceMatrixTriplets.setTriplet( + {berdySensor.range.offset + i, berdySensor.range.offset + i, wrenchCovariance[i]}); + } break; - case iDynTree::BerdySensorTypes::RCM_SENSOR: { + case iDynTree::BerdySensorTypes::RCM_SENSOR: { auto specificMeasurementsPtr = m_extWrenchesEstimator.params.specificMeasurementsCovariance.find("RCM_" "SENS" "OR"); - for (std::size_t i = 0; i < 6; i++) - { - measurementsCovarianceMatrixTriplets.setTriplet( - {berdySensor.range.offset + i, berdySensor.range.offset + i, specificMeasurementsPtr->second[i]}); - } - } - break; - default: + for (std::size_t i = 0; i < 6; i++) + { + measurementsCovarianceMatrixTriplets.setTriplet( + {berdySensor.range.offset + i, berdySensor.range.offset + i, specificMeasurementsPtr->second[i]}); + } + } break; + default: + break; } } + + // Set the prior covariance matrix for measurements iDynTree::SparseMatrix measurementsPriorCovarianceMatrix; std::size_t sigmaYSize = m_extWrenchesEstimator.berdyHelper.getNrOfSensorsMeasurements(); measurementsPriorCovarianceMatrix.resize(sigmaYSize, sigmaYSize); @@ -691,14 +747,14 @@ bool HumanID::initializeExtWrenchesHelper(const std::shared_ptrsetMeasurementsPriorCovariance(measurementsPriorCovarianceMatrix); - // Set mu_d + // Set mu_d (expected value for dynamics regularization) iDynTree::VectorDynSize dynamicsRegularizationExpectedValueVector; dynamicsRegularizationExpectedValueVector.resize(m_extWrenchesEstimator.berdyHelper.getNrOfDynamicVariables()); for (std::size_t i = 0; i < dynamicsRegularizationExpectedValueVector.size(); i++) dynamicsRegularizationExpectedValueVector.setVal(i, m_extWrenchesEstimator.params.priorDynamicsRegularizationExpected); m_extWrenchesEstimator.berdySolver->setDynamicsRegularizationPriorExpectedValue(dynamicsRegularizationExpectedValueVector); - // set Sigma_d + // Set Sigma_d (covariance matrix for dynamics regularization) iDynTree::Triplets priorDynamicsRegularizationCovarianceMatrixTriplets; std::size_t sigmaDSize = m_extWrenchesEstimator.berdyHelper.getNrOfDynamicVariables(); for (std::size_t i = 0; i < sigmaDSize; i++) @@ -711,6 +767,7 @@ bool HumanID::initializeExtWrenchesHelper(const std::shared_ptrsetDynamicsRegularizationPriorCovariance(priorDynamicsRegularizationCovarianceMatrix); + // Check validity of BerdySolver initialization if (!m_extWrenchesEstimator.berdySolver->isValid()) { BiomechanicalAnalysis::log()->error("{} Error in the initialization of the BerdySolver.", logPrefix); @@ -722,6 +779,7 @@ bool HumanID::initializeExtWrenchesHelper(const std::shared_ptrgetCenterOfMassPosition() - m_kinDyn->getWorldBaseTransform().getPosition()); base_H_centroidal.setRotation(m_kinDyn->getWorldBaseTransform().getRotation().inverse()); + + // Compute RCM wrench in base frame rcmWrench = rcmWrench + base_H_centroidal * subjectWeightInCentroidal; return rcmWrench;