From e50cde0d7ba1ce2e8dbc0674a612f7eae2ff6110 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Tue, 24 May 2022 11:48:23 +0200 Subject: [PATCH 01/31] Read NCWE measurement parameters --- conf/xml/Human.xml | 9 ++ devices/HumanWrenchProvider/CMakeLists.txt | 1 + .../HumanWrenchProvider.cpp | 133 +++++++++++++++++- 3 files changed, 142 insertions(+), 1 deletion(-) diff --git a/conf/xml/Human.xml b/conf/xml/Human.xml index 4262401b8..d8dc89906 100644 --- a/conf/xml/Human.xml +++ b/conf/xml/Human.xml @@ -161,6 +161,15 @@ dummy (0.0 0.0 0.0 0.0 0.0 0.0) + + true + + 1e-6 + (LeftFoot RightFoot) + (0.0589998348 0.0589998348 0.0359999712 0.002250000225 0.002250000225 0.56e-3) + (0.0589998348 0.0589998348 0.0359999712 0.002250000225 0.002250000225 0.56e-3) + + FTSourcesIWearRemapper diff --git a/devices/HumanWrenchProvider/CMakeLists.txt b/devices/HumanWrenchProvider/CMakeLists.txt index eb810cf49..e4c6ea1d3 100644 --- a/devices/HumanWrenchProvider/CMakeLists.txt +++ b/devices/HumanWrenchProvider/CMakeLists.txt @@ -33,6 +33,7 @@ target_link_libraries(HumanWrenchProvider PUBLIC IWear::IWear iDynTree::idyntree-core iDynTree::idyntree-model + iDynTree::idyntree-estimation iDynTree::idyntree-high-level) yarp_install( diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index c29bad808..1fa98b7e9 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -19,6 +19,8 @@ #include #include #include +#include +#include #include #include @@ -27,6 +29,7 @@ #include #include #include +#include const std::string DeviceName = "HumanWrenchProvider"; const std::string LogPrefix = DeviceName + " :"; @@ -54,6 +57,19 @@ enum rpcCommand setWorldWrench, }; +/** + * This structure holds the parameters used for the Non-Collocated Wrench Estimation. + */ +struct MAPEstParams +{ + bool useMAPEst; + iDynTree::VectorDynSize priorDynamicsRegularizationExpected; // mu_d + iDynTree::SparseMatrix priorMeasurementsCovariance; // Sigma_y + // measurements + double measurementDefaultCovariance; + std::unordered_map> specificMeasurementsCovariance; +}; + struct WrenchSourceData { std::string name; @@ -125,6 +141,12 @@ class HumanWrenchProvider::Impl std::unique_ptr commandPro; yarp::os::RpcServer rpcPort; bool applyRpcCommand(); + + // MAP estimator variables + MAPEstParams mapEstParams; + iDynTree::BerdyHelper berdyHelper; + std::unique_ptr mapSolver; + std::vector transformedWrenches; }; HumanWrenchProvider::HumanWrenchProvider() @@ -234,6 +256,102 @@ bool parseWrench(yarp::os::Bottle* list, iDynTree::Wrench& wrench) wrench = iDynTree::Wrench(iDynTree::Force(list->get(0).asFloat64(), list->get(1).asFloat64(), list->get(2).asFloat64()), iDynTree::Torque(list->get(3).asFloat64(), list->get(4).asFloat64(), list->get(5).asFloat64())); + + return true; +} + +bool parseMeasurementsCovariance(yarp::os::Searchable& config, MAPEstParams& mapEstParams) +{ + // find the group cov_measurements_NET_EXT_WRENCH_SENSOR + yarp::os::Bottle& priorMeasurementsCovarianceBottle = config.findGroup("cov_measurements_NET_EXT_WRENCH_SENSOR"); + if(priorMeasurementsCovarianceBottle.isNull()) + { + //TODO use default values? + return false; + } + + // find the default value + if(!priorMeasurementsCovarianceBottle.check("value")) + { + //TODO + return false; + } + mapEstParams.measurementDefaultCovariance = priorMeasurementsCovarianceBottle.find("value").asFloat64(); + + // find the elements with specific covariance + yarp::os::Value& specificElements = priorMeasurementsCovarianceBottle.find("specificElements"); + if(specificElements.isNull()) + { + yInfo()<<"No specific elements to specify the measurements covariance were found! Using"<size(); i++) + { + std::string linkName = specificElementsList->get(i).asString(); + yDebug()<size()!=6) + { + yError()<<"The covariance values of"< covarianceListValues; + for(int j=0; jsize(); j++) + { + covarianceListValues.push_back(covarianceList->get(j).asFloat64()); + } + mapEstParams.specificMeasurementsCovariance.emplace(linkName, covarianceListValues); + + } + return true; +} + +bool parseMAPEstParams(yarp::os::Searchable& config, MAPEstParams& mapEstParams) +{ + // find the param group + yarp::os::Bottle& mapEstParamsGroup = config.findGroup("MAPEstParams"); + if(mapEstParamsGroup.isNull()) + { + mapEstParams.useMAPEst = false; + return true; + } + + // parse the useMAP + if(!mapEstParamsGroup.check("useMAPEst")) + { + yError() << "Missing useMAPEst parameter!"; + return false; + } + mapEstParams.useMAPEst = mapEstParamsGroup.find("useMAPEst").asBool(); + + // parse the prior dynamic variable covariance + //TODO mapEstParams.priorDynamicsRegularizationExpected + + // parse the prior measurements covariance Sigma_y + if(!parseMeasurementsCovariance(mapEstParamsGroup, mapEstParams)) + { + return false; + } + return true; } @@ -590,6 +708,12 @@ bool HumanWrenchProvider::open(yarp::os::Searchable& config) // Store the wrench source data pImpl->wrenchSources.emplace_back(WrenchSourceData); } + pImpl->transformedWrenches.resize(pImpl->wrenchSources.size()); + + if(!parseMAPEstParams(config, pImpl->mapEstParams)) + { + return false; + } // =================== // INITIALIZE RPC PORT @@ -806,9 +930,11 @@ void HumanWrenchProvider::run() return; } + pImpl->transformedWrenches[i] = transformedWrench; + // Expose the data as IAnalogSensor // ================================ - { + if(!pImpl->mapEstParams.useMAPEst){ std::lock_guard lock(pImpl->mutex); pImpl->analogSensorData.measurements[6 * i + 0] = transformedWrench.getLinearVec3()(0); @@ -820,6 +946,11 @@ void HumanWrenchProvider::run() } } + if(pImpl->mapEstParams.useMAPEst) + { + //TODO use MAP estimator + } + // Check for rpc command status and apply command if (pImpl->commandPro->cmdStatus != rpcCommand::empty) { // Apply rpc command From 30e4ed685fa1b1c8a0bdaa9df8d52c05bda10158 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Tue, 24 May 2022 18:12:07 +0200 Subject: [PATCH 02/31] Read mu_dyn_variables param --- .../HumanWrenchProvider.cpp | 28 +++++++++++++------ 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index 1fa98b7e9..de930f239 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -63,9 +63,10 @@ enum rpcCommand struct MAPEstParams { bool useMAPEst; - iDynTree::VectorDynSize priorDynamicsRegularizationExpected; // mu_d iDynTree::SparseMatrix priorMeasurementsCovariance; // Sigma_y - // measurements + // dynamic variables params + double priorDynamicsRegularizationExpected; // mu_d + // measurements params double measurementDefaultCovariance; std::unordered_map> specificMeasurementsCovariance; }; @@ -295,7 +296,6 @@ bool parseMeasurementsCovariance(yarp::os::Searchable& config, MAPEstParams& map for(int i=0; isize(); i++) { std::string linkName = specificElementsList->get(i).asString(); - yDebug()< Date: Mon, 20 Jun 2022 16:31:29 +0200 Subject: [PATCH 03/31] Initialize MAP estimation solver --- .../HumanWrenchProvider.cpp | 74 ++++++++++++++++++- 1 file changed, 72 insertions(+), 2 deletions(-) diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index de930f239..a3cdf4892 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -71,6 +71,13 @@ struct MAPEstParams std::unordered_map> specificMeasurementsCovariance; }; +struct MAPEstHelper +{ + iDynTree::BerdyOptions berdyOptions; + iDynTree::BerdyHelper berdyHelper; + std::unique_ptr mapSolver; +}; + struct WrenchSourceData { std::string name; @@ -145,8 +152,7 @@ class HumanWrenchProvider::Impl // MAP estimator variables MAPEstParams mapEstParams; - iDynTree::BerdyHelper berdyHelper; - std::unique_ptr mapSolver; + MAPEstHelper mapEstHelper; std::vector transformedWrenches; }; @@ -725,6 +731,70 @@ bool HumanWrenchProvider::open(yarp::os::Searchable& config) return false; } + // Configure map estimator + if(pImpl->mapEstParams.useMAPEst) + { + pImpl->mapEstHelper.berdyOptions.berdyVariant = iDynTree::BerdyVariants::BERDY_FLOATING_BASE_NON_COLLOCATED_EXT_WRENCHES; + pImpl->mapEstHelper.berdyOptions.includeAllNetExternalWrenchesAsSensors = true; + pImpl->mapEstHelper.berdyOptions.includeRcmAsSensor = true; + pImpl->mapEstHelper.berdyOptions.includeAllJointTorquesAsSensors = false; + pImpl->mapEstHelper.berdyOptions.includeAllJointAccelerationsAsSensors = false; + pImpl->mapEstHelper.berdyOptions.includeAllNetExternalWrenchesAsSensors = true; + pImpl->mapEstHelper.berdyOptions.includeAllNetExternalWrenchesAsDynamicVariables = true; + + pImpl->mapEstHelper.berdyOptions.baseLink = "Pelvis"; //TODO use the ihumanstate one + + if(!pImpl->mapEstHelper.berdyOptions.checkConsistency()) + { + yError() << LogPrefix << "BERDY options are not consistent"; + return false; + } + + // Initialize the BerdyHelper + if (!pImpl->mapEstHelper.berdyHelper.init(humanModelLoader.model(), iDynTree::SensorsList(), pImpl->mapEstHelper.berdyOptions)) { + yError() << LogPrefix << "Failed to initialize BERDY"; + return false; + } + + pImpl->mapEstHelper.mapSolver = std::make_unique(pImpl->mapEstHelper.berdyHelper); + pImpl->mapEstHelper.mapSolver->initialize(); + + if (!pImpl->mapEstHelper.mapSolver->isValid()) { + yError() << LogPrefix << "Failed to initialize the Berdy MAP solver"; + return false; + } + + //TODO configure solver + + // sigma_y + iDynTree::SparseMatrix measurementsCovarianceMatrix; + for (const iDynTree::BerdySensor& berdySensor : pImpl->mapEstHelper.berdyHelper.getSensorsOrdering()) { + switch(berdySensor.type) + { + case iDynTree::BerdySensorTypes::NET_EXT_WRENCH_SENSOR: + //TODO + //yInfo()<berdyData.solver->setDynamicsRegularizationPriorExpectedValue(pImpl->berdyData.priors.task1_dynamicsRegularizationExpectedValueVector, pImpl->task1); + //yInfo() << LogPrefix << "Task1 Berdy solver DynamicsRegularizationPriorExpectedValue set successfully"; +// + //pImpl->berdyData.solver->setDynamicsRegularizationPriorCovariance(pImpl->berdyData.priors.task1_dynamicsRegularizationCovarianceMatrix, pImpl->task1); + //yInfo() << LogPrefix << "Task1 Berdy solver DynamicsRegularizationPriorCovariance set successfully"; +// + //pImpl->berdyData.solver->setMeasurementsPriorCovariance(pImpl->berdyData.priors.task1_measurementsCovarianceMatrix, pImpl->task1); + //yInfo() << LogPrefix << "Task1 Berdy solver MeasurementsPriorCovariance set successfully"; + } + // =================== // INITIALIZE RPC PORT // =================== From 63e103c3b7c27f317421085062dea080d95c4121 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Tue, 21 Jun 2022 10:18:53 +0200 Subject: [PATCH 04/31] Initialize NET_EXT_WRENCH_SENSOR sensor covariance --- .../HumanWrenchProvider/HumanWrenchProvider.cpp | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index a3cdf4892..08349c34c 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -767,14 +767,25 @@ bool HumanWrenchProvider::open(yarp::os::Searchable& config) //TODO configure solver // sigma_y - iDynTree::SparseMatrix measurementsCovarianceMatrix; + iDynTree::Triplets measurementsCovarianceMatrix; for (const iDynTree::BerdySensor& berdySensor : pImpl->mapEstHelper.berdyHelper.getSensorsOrdering()) { switch(berdySensor.type) { case iDynTree::BerdySensorTypes::NET_EXT_WRENCH_SENSOR: - //TODO - //yInfo()<mapEstParams.measurementDefaultCovariance); + + // set specific covariance if configured + auto specificMeasurementsPtr = pImpl->mapEstParams.specificMeasurementsCovariance.find(berdySensor.id); + if(specificMeasurementsPtr!=pImpl->mapEstParams.specificMeasurementsCovariance.end()) + { + for(int i=0; i<6; i++) wrenchCovariance.setVal(i, specificMeasurementsPtr->second[i]); + } + for(int i=0; i<6; i++) measurementsCovarianceMatrix.setTriplet({berdySensor.range.offset+i,berdySensor.range.offset+i, wrenchCovariance[i]}); + } break; case iDynTree::BerdySensorTypes::RCM_SENSOR: //TODO From efd97ce4e43e5cfcf852067291c530ed1a7571e0 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Tue, 21 Jun 2022 10:46:44 +0200 Subject: [PATCH 05/31] Set RCM_SENSOR covariance --- .../HumanWrenchProvider.cpp | 48 +++++++++++++++++-- 1 file changed, 43 insertions(+), 5 deletions(-) diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index 08349c34c..92e80a138 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -328,6 +328,37 @@ bool parseMeasurementsCovariance(yarp::os::Searchable& config, MAPEstParams& map mapEstParams.specificMeasurementsCovariance.emplace(linkName, covarianceListValues); } + + // get the RCM sensor covariance (if the specified) + yarp::os::Value& rcmPriorMeasurementsCovarianceValue = config.find("cov_measurements_RCM_SENSOR"); + std::vector rcmCovariance; + if(rcmPriorMeasurementsCovarianceValue.isNull()) + { + yInfo()<size()!=6) + { + yError()<size(); i++) + { + rcmCovariance.push_back(rcmPriorMeasurementsCovarianceList->get(i).asFloat64()); + } + } + + mapEstParams.specificMeasurementsCovariance.emplace("RCM_SENSOR", rcmCovariance); + return true; } @@ -767,7 +798,7 @@ bool HumanWrenchProvider::open(yarp::os::Searchable& config) //TODO configure solver // sigma_y - iDynTree::Triplets measurementsCovarianceMatrix; + iDynTree::Triplets measurementsCovarianceMatrixTriplets; for (const iDynTree::BerdySensor& berdySensor : pImpl->mapEstHelper.berdyHelper.getSensorsOrdering()) { switch(berdySensor.type) { @@ -784,16 +815,26 @@ bool HumanWrenchProvider::open(yarp::os::Searchable& config) for(int i=0; i<6; i++) wrenchCovariance.setVal(i, specificMeasurementsPtr->second[i]); } - for(int i=0; i<6; i++) measurementsCovarianceMatrix.setTriplet({berdySensor.range.offset+i,berdySensor.range.offset+i, wrenchCovariance[i]}); + 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: //TODO + { + auto specificMeasurementsPtr = pImpl->mapEstParams.specificMeasurementsCovariance.find("RCM_SENSOR"); + 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; } } + iDynTree::SparseMatrix measurementsPriorCovarianceMatrix; + measurementsPriorCovarianceMatrix.setFromTriplets(measurementsCovarianceMatrixTriplets); + pImpl->mapEstHelper.mapSolver->setMeasurementsPriorCovariance(measurementsPriorCovarianceMatrix); // Set the priors to berdy solver for task1 //pImpl->berdyData.solver->setDynamicsRegularizationPriorExpectedValue(pImpl->berdyData.priors.task1_dynamicsRegularizationExpectedValueVector, pImpl->task1); @@ -801,9 +842,6 @@ bool HumanWrenchProvider::open(yarp::os::Searchable& config) // //pImpl->berdyData.solver->setDynamicsRegularizationPriorCovariance(pImpl->berdyData.priors.task1_dynamicsRegularizationCovarianceMatrix, pImpl->task1); //yInfo() << LogPrefix << "Task1 Berdy solver DynamicsRegularizationPriorCovariance set successfully"; -// - //pImpl->berdyData.solver->setMeasurementsPriorCovariance(pImpl->berdyData.priors.task1_measurementsCovarianceMatrix, pImpl->task1); - //yInfo() << LogPrefix << "Task1 Berdy solver MeasurementsPriorCovariance set successfully"; } // =================== From f90d25f0c3777799496bde711c9df2ddd9fc40f6 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Tue, 21 Jun 2022 12:39:14 +0200 Subject: [PATCH 06/31] Set NCWE mu_d --- devices/HumanWrenchProvider/HumanWrenchProvider.cpp | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index 92e80a138..b25526587 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -795,8 +795,6 @@ bool HumanWrenchProvider::open(yarp::os::Searchable& config) return false; } - //TODO configure solver - // sigma_y iDynTree::Triplets measurementsCovarianceMatrixTriplets; for (const iDynTree::BerdySensor& berdySensor : pImpl->mapEstHelper.berdyHelper.getSensorsOrdering()) { @@ -836,12 +834,11 @@ bool HumanWrenchProvider::open(yarp::os::Searchable& config) measurementsPriorCovarianceMatrix.setFromTriplets(measurementsCovarianceMatrixTriplets); pImpl->mapEstHelper.mapSolver->setMeasurementsPriorCovariance(measurementsPriorCovarianceMatrix); - // Set the priors to berdy solver for task1 - //pImpl->berdyData.solver->setDynamicsRegularizationPriorExpectedValue(pImpl->berdyData.priors.task1_dynamicsRegularizationExpectedValueVector, pImpl->task1); - //yInfo() << LogPrefix << "Task1 Berdy solver DynamicsRegularizationPriorExpectedValue set successfully"; -// - //pImpl->berdyData.solver->setDynamicsRegularizationPriorCovariance(pImpl->berdyData.priors.task1_dynamicsRegularizationCovarianceMatrix, pImpl->task1); - //yInfo() << LogPrefix << "Task1 Berdy solver DynamicsRegularizationPriorCovariance set successfully"; + // Set mu_d + iDynTree::VectorDynSize dynamicsRegularizationExpectedValueVector; + dynamicsRegularizationExpectedValueVector.resize(6); + for(std::size_t i=0; i<6; i++) dynamicsRegularizationExpectedValueVector.setVal(i, pImpl->mapEstParams.priorDynamicsRegularizationExpected); + pImpl->mapEstHelper.mapSolver->setDynamicsRegularizationPriorExpectedValue(dynamicsRegularizationExpectedValueVector); } // =================== From 3457eb5aac0131a7db6cdff116b43aa486cca2c6 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Tue, 21 Jun 2022 16:09:44 +0200 Subject: [PATCH 07/31] Fix NCWE measurementsPriorCovarianceMatrix init --- devices/HumanWrenchProvider/HumanWrenchProvider.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index b25526587..8d536a848 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -831,6 +831,8 @@ bool HumanWrenchProvider::open(yarp::os::Searchable& config) } } iDynTree::SparseMatrix measurementsPriorCovarianceMatrix; + std::size_t sigmaYSize = pImpl->mapEstHelper.berdyHelper.getNrOfSensorsMeasurements(); + measurementsPriorCovarianceMatrix.resize(sigmaYSize, sigmaYSize); measurementsPriorCovarianceMatrix.setFromTriplets(measurementsCovarianceMatrixTriplets); pImpl->mapEstHelper.mapSolver->setMeasurementsPriorCovariance(measurementsPriorCovarianceMatrix); From 03b64003fb007ebd4a3acca80c811d2050c6e742 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Tue, 21 Jun 2022 16:35:24 +0200 Subject: [PATCH 08/31] Set NCWE priorDynamicsRegularizationCovariance --- .../HumanWrenchProvider.cpp | 26 ++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index 8d536a848..b151faafd 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -64,8 +64,8 @@ struct MAPEstParams { bool useMAPEst; iDynTree::SparseMatrix priorMeasurementsCovariance; // Sigma_y - // dynamic variables params double priorDynamicsRegularizationExpected; // mu_d + double priorDynamicsRegularizationCovarianceValue; // Sigma_d // measurements params double measurementDefaultCovariance; std::unordered_map> specificMeasurementsCovariance; @@ -392,6 +392,14 @@ bool parseMAPEstParams(yarp::os::Searchable& config, MAPEstParams& mapEstParams) } mapEstParams.priorDynamicsRegularizationExpected = mapEstParamsGroup.find("mu_dyn_variables").asFloat64(); + if(!mapEstParamsGroup.check("cov_dyn_variables") || + !mapEstParamsGroup.find("cov_dyn_variables").isFloat64()) + { + yError() << "Missing valid floating point cov_dyn_variables parameter!"; + return false; + } + mapEstParams.priorDynamicsRegularizationExpected = mapEstParamsGroup.find("cov_dyn_variables").asFloat64(); + // parse the prior measurements covariance Sigma_y if(!parseMeasurementsCovariance(mapEstParamsGroup, mapEstParams)) { @@ -838,9 +846,21 @@ bool HumanWrenchProvider::open(yarp::os::Searchable& config) // Set mu_d iDynTree::VectorDynSize dynamicsRegularizationExpectedValueVector; - dynamicsRegularizationExpectedValueVector.resize(6); - for(std::size_t i=0; i<6; i++) dynamicsRegularizationExpectedValueVector.setVal(i, pImpl->mapEstParams.priorDynamicsRegularizationExpected); + dynamicsRegularizationExpectedValueVector.resize(pImpl->mapEstHelper.berdyHelper.getNrOfDynamicVariables()); + for(std::size_t i=0; imapEstParams.priorDynamicsRegularizationExpected); pImpl->mapEstHelper.mapSolver->setDynamicsRegularizationPriorExpectedValue(dynamicsRegularizationExpectedValueVector); + + // set Sigma_d + iDynTree::Triplets priorDynamicsRegularizationCovarianceMatrixTriplets; + std::size_t sigmaDSize = pImpl->mapEstHelper.berdyHelper.getNrOfDynamicVariables(); + for(std::size_t i=0; imapEstParams.priorDynamicsRegularizationCovarianceValue}); + } + iDynTree::SparseMatrix priorDynamicsRegularizationCovarianceMatrix; + priorDynamicsRegularizationCovarianceMatrix.resize(sigmaDSize, sigmaDSize); + priorDynamicsRegularizationCovarianceMatrix.setFromTriplets(priorDynamicsRegularizationCovarianceMatrixTriplets); + pImpl->mapEstHelper.mapSolver->setDynamicsRegularizationPriorCovariance(priorDynamicsRegularizationCovarianceMatrix); } // =================== From 1b804b1dc629740cf7d574fc53672e13d7bdc537 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Tue, 21 Jun 2022 17:03:11 +0200 Subject: [PATCH 09/31] Populate NCWE measurements vector --- .../HumanWrenchProvider.cpp | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index b151faafd..a4facc999 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -1096,6 +1096,33 @@ void HumanWrenchProvider::run() if(pImpl->mapEstParams.useMAPEst) { + // Set NET_EXT_WRENCH measurements + iDynTree::VectorDynSize berdyMeasurementVector(pImpl->mapEstHelper.berdyHelper.getNrOfDynamicVariables()); + berdyMeasurementVector.zero(); + for (unsigned i = 0; i < pImpl->wrenchSources.size(); ++i) { + auto& forceSource = pImpl->wrenchSources[i]; + std::string sensorName = forceSource.outputFrame; + iDynTree::LinkIndex linkIndex = pImpl->mapEstHelper.berdyHelper.model().getLinkIndex(sensorName); + if(linkIndex==iDynTree::LINK_INVALID_INDEX) + { + yError()<mapEstHelper.berdyHelper.getRangeLinkSensorVariable(iDynTree::BerdySensorTypes::NET_EXT_WRENCH_SENSOR, linkIndex); + for(std::size_t j=0; jtransformedWrenches[i].getVal(j)); + } + } + + // Set RCM_SENSOR measurement + iDynTree::SpatialForceVector rcm; //TODO compute rcm + iDynTree::IndexRange rcmSensorRange = pImpl->mapEstHelper.berdyHelper.getRangeRCMSensorVariable(iDynTree::BerdySensorTypes::RCM_SENSOR); + for(std::size_t j=0; j Date: Tue, 21 Jun 2022 18:09:33 +0200 Subject: [PATCH 10/31] Update exposed data with estimate --- .../HumanWrenchProvider.cpp | 75 +++++++++++++++---- 1 file changed, 60 insertions(+), 15 deletions(-) diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index a4facc999..f26e54952 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -1079,25 +1079,12 @@ void HumanWrenchProvider::run() } pImpl->transformedWrenches[i] = transformedWrench; - - // Expose the data as IAnalogSensor - // ================================ - if(!pImpl->mapEstParams.useMAPEst){ - std::lock_guard lock(pImpl->mutex); - - pImpl->analogSensorData.measurements[6 * i + 0] = transformedWrench.getLinearVec3()(0); - pImpl->analogSensorData.measurements[6 * i + 1] = transformedWrench.getLinearVec3()(1); - pImpl->analogSensorData.measurements[6 * i + 2] = transformedWrench.getLinearVec3()(2); - pImpl->analogSensorData.measurements[6 * i + 3] = transformedWrench.getAngularVec3()(0); - pImpl->analogSensorData.measurements[6 * i + 4] = transformedWrench.getAngularVec3()(1); - pImpl->analogSensorData.measurements[6 * i + 5] = transformedWrench.getAngularVec3()(2); - } } if(pImpl->mapEstParams.useMAPEst) { // Set NET_EXT_WRENCH measurements - iDynTree::VectorDynSize berdyMeasurementVector(pImpl->mapEstHelper.berdyHelper.getNrOfDynamicVariables()); + iDynTree::VectorDynSize berdyMeasurementVector(pImpl->mapEstHelper.berdyHelper.getNrOfSensorsMeasurements()); berdyMeasurementVector.zero(); for (unsigned i = 0; i < pImpl->wrenchSources.size(); ++i) { auto& forceSource = pImpl->wrenchSources[i]; @@ -1117,13 +1104,71 @@ void HumanWrenchProvider::run() // Set RCM_SENSOR measurement iDynTree::SpatialForceVector rcm; //TODO compute rcm + rcm.zero(); iDynTree::IndexRange rcmSensorRange = pImpl->mapEstHelper.berdyHelper.getRangeRCMSensorVariable(iDynTree::BerdySensorTypes::RCM_SENSOR); for(std::size_t j=0; jmapEstHelper.berdyHelper.model(); + + iDynTree::Vector3 baseAngularVelocity; + iDynTree::JointPosDoubleArray jointsPosition(model); + iDynTree::JointDOFsDoubleArray jointsVelocity(model); + iDynTree::JointDOFsDoubleArray jointsAcceleration(model); + iDynTree::FrameIndex baseFrameIndex = model.getLinkIndex(pImpl->mapEstHelper.berdyOptions.baseLink); + + for(int i=0; ijointPositionsInterface.size(); i++) jointsPosition.setVal(i, pImpl->jointPositionsInterface.at(i)); + for(int i=0; ijointVelocitiesInterface.size(); i++) jointsVelocity.setVal(i, pImpl->jointVelocitiesInterface.at(i)); + for(int i=0; i<3; i++) baseAngularVelocity.setVal(i, pImpl->baseVelocityInterface.at(i+3)); + + + // Set the kinematic information necessary for the dynamics estimation + //TODO check if needed + pImpl->mapEstHelper.berdyHelper.updateKinematicsFromFloatingBase(jointsPosition, + jointsVelocity, + baseFrameIndex, + baseAngularVelocity); + + + pImpl->mapEstHelper.mapSolver->updateEstimateInformationFloatingBase(jointsPosition, + jointsVelocity, + baseFrameIndex, + baseAngularVelocity, + berdyMeasurementVector); + + + if(!pImpl->mapEstHelper.mapSolver->doEstimate()) + { + yError()<mapEstHelper.mapSolver->getLastEstimate(); + + // update transformed wrenches with estimate + for (unsigned i = 0; i < pImpl->wrenchSources.size(); ++i) + { + for(int j=0; j<6; j++) + { + pImpl->transformedWrenches[i].setVal(j, estimatedWrenches.getVal(j)); + } + } + } + + // Expose the data as IAnalogSensor + { + std::lock_guard lock(pImpl->mutex); + for (unsigned i = 0; i < pImpl->wrenchSources.size(); ++i) { + pImpl->analogSensorData.measurements[6 * i + 0] = pImpl->transformedWrenches[i].getLinearVec3()(0); + pImpl->analogSensorData.measurements[6 * i + 1] = pImpl->transformedWrenches[i].getLinearVec3()(1); + pImpl->analogSensorData.measurements[6 * i + 2] = pImpl->transformedWrenches[i].getLinearVec3()(2); + pImpl->analogSensorData.measurements[6 * i + 3] = pImpl->transformedWrenches[i].getAngularVec3()(0); + pImpl->analogSensorData.measurements[6 * i + 4] = pImpl->transformedWrenches[i].getAngularVec3()(1); + pImpl->analogSensorData.measurements[6 * i + 5] = pImpl->transformedWrenches[i].getAngularVec3()(2); + } } // Check for rpc command status and apply command From b4d836c9c2675b04f4ab73a2c20f84cc45251f33 Mon Sep 17 00:00:00 2001 From: Lorenzo Rapetti Date: Tue, 21 Jun 2022 11:37:09 +0200 Subject: [PATCH 11/31] Update Human.xml --- conf/xml/Human.xml | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/conf/xml/Human.xml b/conf/xml/Human.xml index d8dc89906..0a860d2c9 100644 --- a/conf/xml/Human.xml +++ b/conf/xml/Human.xml @@ -170,6 +170,19 @@ (0.0589998348 0.0589998348 0.0359999712 0.002250000225 0.002250000225 0.56e-3) + + FTSourcesIWearRemapper From ec5f076e0179dda4ef2b0da4998d3051e6c05a49 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Wed, 22 Jun 2022 15:37:13 +0200 Subject: [PATCH 12/31] Add computation of RCM sensor measurement --- .../HumanWrenchProvider.cpp | 95 ++++++++++++++++--- 1 file changed, 81 insertions(+), 14 deletions(-) diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index f26e54952..4c1190123 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -410,6 +410,65 @@ bool parseMAPEstParams(yarp::os::Searchable& config, MAPEstParams& mapEstParams) return true; } +iDynTree::SpatialForceVector computeRCMInBaseUsingMeasurements(iDynTree::KinDynComputations& kinDynComputations, + iDynTree::LinkIndex baseIdx) +{ + const iDynTree::Model& model = kinDynComputations.model(); + + iDynTree::LinkPositions linkPos(model); + iDynTree::LinkVelArray linkVels(model); + iDynTree::LinkAccArray linkProperAccs(model); + iDynTree::LinkInertias linkInertias(model); + + // Get links info + for(size_t linkIdx = 0; linkIdx < model.getNrOfLinks(); linkIdx++) + { + linkInertias(linkIdx) = model.getLink(linkIdx)->getInertia(); + linkPos(linkIdx) = kinDynComputations.getWorldTransform(linkIdx); + linkVels(linkIdx) = kinDynComputations.getFrameVel(linkIdx); + + //TODO manage accelerations + linkProperAccs(linkIdx).zero(); + } + + iDynTree::SpatialForceVector rcm; + rcm.zero(); + + // Set centroidal to world transform + iDynTree::Transform world_H_centroidal = iDynTree::Transform(iDynTree::Rotation::Identity(), kinDynComputations.getCenterOfMassPosition()); + + // Iterate over measurements + for(iDynTree::LinkIndex visitedLinkIndex = 0; visitedLinkIndex < model.getNrOfLinks(); visitedLinkIndex++) + { + // Get world_H_link transform + const iDynTree::Transform world_H_link = linkPos(visitedLinkIndex); + + // Compute link to centroidal transform + const iDynTree::Transform centroidal_H_link = world_H_centroidal.inverse() * world_H_link; + + // Get link velocity L_v_A,L + const iDynTree::Twist linkVelocityExpressedInLink = linkVels(visitedLinkIndex); + + // Get link acceleration L_a_A,L + const iDynTree::SpatialAcc linkAccelerationExpressedInLink = linkProperAccs(visitedLinkIndex); + + // velocity contribution + const iDynTree::SpatialForceVector linkInertiaMultipliedVelocity = linkInertias(visitedLinkIndex).multiply(linkVelocityExpressedInLink); + const iDynTree::SpatialForceVector rcmLinkVelContrib = centroidal_H_link * (linkVelocityExpressedInLink.cross(linkInertiaMultipliedVelocity)); + + // acceleration contribution + const iDynTree::SpatialForceVector rcmLinkAccContrib = centroidal_H_link * (linkInertias(visitedLinkIndex).multiply(linkAccelerationExpressedInLink)); + + // update rcm + rcm = rcm + rcmLinkVelContrib + rcmLinkAccContrib; + } + + // base_H_centroidal transform + const iDynTree::Transform base_H_centroidal = kinDynComputations.getWorldBaseTransform().inverse() * world_H_centroidal; + + return base_H_centroidal * rcm; +} + bool HumanWrenchProvider::open(yarp::os::Searchable& config) { //Set gravity vector @@ -1103,28 +1162,36 @@ void HumanWrenchProvider::run() } // Set RCM_SENSOR measurement - iDynTree::SpatialForceVector rcm; //TODO compute rcm - rcm.zero(); + iDynTree::Model &model = pImpl->mapEstHelper.berdyHelper.model(); + iDynTree::FrameIndex baseFrameIndex = model.getLinkIndex(pImpl->mapEstHelper.berdyOptions.baseLink); + + iDynTree::KinDynComputations kinDynComputations; + kinDynComputations.loadRobotModel(model); + kinDynComputations.setFrameVelocityRepresentation(iDynTree::FrameVelocityRepresentation::BODY_FIXED_REPRESENTATION); + kinDynComputations.setRobotState(pImpl->basePose, + pImpl->humanJointPositionsVec, + pImpl->baseVelocity, + pImpl->humanJointVelocitiesVec, + pImpl->world_gravity); + + iDynTree::SpatialForceVector rcm = computeRCMInBaseUsingMeasurements(kinDynComputations, baseFrameIndex); + iDynTree::IndexRange rcmSensorRange = pImpl->mapEstHelper.berdyHelper.getRangeRCMSensorVariable(iDynTree::BerdySensorTypes::RCM_SENSOR); for(std::size_t j=0; jmapEstHelper.berdyHelper.model(); - - iDynTree::Vector3 baseAngularVelocity; + iDynTree::Vector3 baseAngularVelocity = pImpl->baseVelocity.getAngularVec3(); iDynTree::JointPosDoubleArray jointsPosition(model); iDynTree::JointDOFsDoubleArray jointsVelocity(model); iDynTree::JointDOFsDoubleArray jointsAcceleration(model); - iDynTree::FrameIndex baseFrameIndex = model.getLinkIndex(pImpl->mapEstHelper.berdyOptions.baseLink); - for(int i=0; ijointPositionsInterface.size(); i++) jointsPosition.setVal(i, pImpl->jointPositionsInterface.at(i)); - for(int i=0; ijointVelocitiesInterface.size(); i++) jointsVelocity.setVal(i, pImpl->jointVelocitiesInterface.at(i)); - for(int i=0; i<3; i++) baseAngularVelocity.setVal(i, pImpl->baseVelocityInterface.at(i+3)); - + for(int i=0; ihumanJointPositionsVec.size(); i++) jointsPosition.setVal(i, pImpl->humanJointPositionsVec.getVal(i)); + for(int i=0; ihumanJointVelocitiesVec.size(); i++) jointsVelocity.setVal(i, pImpl->humanJointVelocitiesVec.getVal(i)); + + // Set the kinematic information necessary for the dynamics estimation //TODO check if needed pImpl->mapEstHelper.berdyHelper.updateKinematicsFromFloatingBase(jointsPosition, @@ -1135,9 +1202,9 @@ void HumanWrenchProvider::run() pImpl->mapEstHelper.mapSolver->updateEstimateInformationFloatingBase(jointsPosition, jointsVelocity, - baseFrameIndex, - baseAngularVelocity, - berdyMeasurementVector); + baseFrameIndex, + baseAngularVelocity, + berdyMeasurementVector); if(!pImpl->mapEstHelper.mapSolver->doEstimate()) From aa5f1bf88fbdee5c9e8c8eb74f1fedad02546d38 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Fri, 8 Jul 2022 12:30:09 +0200 Subject: [PATCH 13/31] Fix BerdySparseMAPSolver in WrenchProvider --- .../HumanWrenchProvider.cpp | 139 ++++++++++++------ 1 file changed, 90 insertions(+), 49 deletions(-) diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index 4c1190123..7296e421e 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -290,7 +290,6 @@ bool parseMeasurementsCovariance(yarp::os::Searchable& config, MAPEstParams& map if(specificElements.isNull()) { yInfo()<<"No specific elements to specify the measurements covariance were found! Using"<size(); i++) + if(!specificElements.isNull()) { - std::string linkName = specificElementsList->get(i).asString(); - if(!priorMeasurementsCovarianceBottle.check(linkName)) + for(int i=0; isize(); i++) { - yError()<<"The element"<get(i).asString(); + if(!priorMeasurementsCovarianceBottle.check(linkName)) + { + yError()<<"The element"<size()!=6) + { + yError()<<"The covariance values of"< covarianceListValues; + for(int j=0; jsize(); j++) + { + covarianceListValues.push_back(covarianceList->get(j).asFloat64()); + } + mapEstParams.specificMeasurementsCovariance.emplace(linkName, covarianceListValues); - // get the diagonal values of the covariance - yarp::os::Bottle* covarianceList = priorMeasurementsCovarianceBottle.find(linkName).asList(); - if(covarianceList==nullptr) - { - yError()<<"The"<size()!=6) - { - yError()<<"The covariance values of"< covarianceListValues; - for(int j=0; jsize(); j++) - { - covarianceListValues.push_back(covarianceList->get(j).asFloat64()); } - mapEstParams.specificMeasurementsCovariance.emplace(linkName, covarianceListValues); } - // get the RCM sensor covariance (if the specified) + // get the RCM sensor covariance (if specified) yarp::os::Value& rcmPriorMeasurementsCovarianceValue = config.find("cov_measurements_RCM_SENSOR"); std::vector rcmCovariance; if(rcmPriorMeasurementsCovarianceValue.isNull()) @@ -398,7 +401,7 @@ bool parseMAPEstParams(yarp::os::Searchable& config, MAPEstParams& mapEstParams) yError() << "Missing valid floating point cov_dyn_variables parameter!"; return false; } - mapEstParams.priorDynamicsRegularizationExpected = mapEstParamsGroup.find("cov_dyn_variables").asFloat64(); + mapEstParams.priorDynamicsRegularizationCovarianceValue = mapEstParamsGroup.find("cov_dyn_variables").asFloat64(); // parse the prior measurements covariance Sigma_y if(!parseMeasurementsCovariance(mapEstParamsGroup, mapEstParams)) @@ -413,6 +416,24 @@ bool parseMAPEstParams(yarp::os::Searchable& config, MAPEstParams& mapEstParams) iDynTree::SpatialForceVector computeRCMInBaseUsingMeasurements(iDynTree::KinDynComputations& kinDynComputations, iDynTree::LinkIndex baseIdx) { + //TODO + iDynTree::SpatialForceVector rcm; + rcm.zero(); + //return rcm; + + iDynTree::SpatialForceVector subjectWeight; + subjectWeight.zero(); + subjectWeight.setVal(2, 9.81 * kinDynComputations.model().getTotalMass()); //TODO set weight from parameter + yDebug()<mapEstParams.useMAPEst) { + pImpl->readHumanData = true; + pImpl->mapEstHelper.berdyOptions.berdyVariant = iDynTree::BerdyVariants::BERDY_FLOATING_BASE_NON_COLLOCATED_EXT_WRENCHES; pImpl->mapEstHelper.berdyOptions.includeAllNetExternalWrenchesAsSensors = true; - pImpl->mapEstHelper.berdyOptions.includeRcmAsSensor = true; pImpl->mapEstHelper.berdyOptions.includeAllJointTorquesAsSensors = false; pImpl->mapEstHelper.berdyOptions.includeAllJointAccelerationsAsSensors = false; pImpl->mapEstHelper.berdyOptions.includeAllNetExternalWrenchesAsSensors = true; @@ -849,7 +868,7 @@ bool HumanWrenchProvider::open(yarp::os::Searchable& config) } // Initialize the BerdyHelper - if (!pImpl->mapEstHelper.berdyHelper.init(humanModelLoader.model(), iDynTree::SensorsList(), pImpl->mapEstHelper.berdyOptions)) { + if (!pImpl->mapEstHelper.berdyHelper.init(pImpl->humanModel, iDynTree::SensorsList(), pImpl->mapEstHelper.berdyOptions)) { yError() << LogPrefix << "Failed to initialize BERDY"; return false; } @@ -857,11 +876,6 @@ bool HumanWrenchProvider::open(yarp::os::Searchable& config) pImpl->mapEstHelper.mapSolver = std::make_unique(pImpl->mapEstHelper.berdyHelper); pImpl->mapEstHelper.mapSolver->initialize(); - if (!pImpl->mapEstHelper.mapSolver->isValid()) { - yError() << LogPrefix << "Failed to initialize the Berdy MAP solver"; - return false; - } - // sigma_y iDynTree::Triplets measurementsCovarianceMatrixTriplets; for (const iDynTree::BerdySensor& berdySensor : pImpl->mapEstHelper.berdyHelper.getSensorsOrdering()) { @@ -884,7 +898,6 @@ bool HumanWrenchProvider::open(yarp::os::Searchable& config) } break; case iDynTree::BerdySensorTypes::RCM_SENSOR: - //TODO { auto specificMeasurementsPtr = pImpl->mapEstParams.specificMeasurementsCovariance.find("RCM_SENSOR"); for(std::size_t i=0; i<6; i++) @@ -920,6 +933,12 @@ bool HumanWrenchProvider::open(yarp::os::Searchable& config) priorDynamicsRegularizationCovarianceMatrix.resize(sigmaDSize, sigmaDSize); priorDynamicsRegularizationCovarianceMatrix.setFromTriplets(priorDynamicsRegularizationCovarianceMatrixTriplets); pImpl->mapEstHelper.mapSolver->setDynamicsRegularizationPriorCovariance(priorDynamicsRegularizationCovarianceMatrix); + + if (!pImpl->mapEstHelper.mapSolver->isValid()) { + yError() << LogPrefix << "Failed to initialize the Berdy MAP solver"; + return false; + } + } // =================== @@ -942,6 +961,16 @@ bool HumanWrenchProvider::open(yarp::os::Searchable& config) // Set rpc port reader pImpl->rpcPort.setReader(*pImpl->commandPro); + // Initialize buffer data + pImpl->baseVelocity.zero(); + + // Resize human joint quantities buffer + pImpl->humanJointPositionsVec.resize(pImpl->humanModel.getNrOfDOFs()); + pImpl->humanJointVelocitiesVec.resize(pImpl->humanModel.getNrOfDOFs()); + + pImpl->humanJointPositionsVec.zero(); + pImpl->humanJointVelocitiesVec.zero(); + return true; } @@ -953,6 +982,12 @@ bool HumanWrenchProvider::close() void HumanWrenchProvider::run() { if (pImpl->readHumanData) { + if(pImpl->iHumanState==nullptr) + { + yError()<iHumanState->getBasePosition(); auto baseVelocityInterface = pImpl->iHumanState->getBaseVelocity(); auto baseOrientationInterface = pImpl->iHumanState->getBaseOrientation(); @@ -960,10 +995,6 @@ void HumanWrenchProvider::run() auto jointVelocitiesInterface = pImpl->iHumanState->getJointVelocities(); auto jointNamesStateInterface = pImpl->iHumanState->getJointNames(); - // Resize human joint quantities buffer - pImpl->humanJointPositionsVec.resize(pImpl->humanModel.getNrOfDOFs()); - pImpl->humanJointVelocitiesVec.resize(pImpl->humanModel.getNrOfDOFs()); - pImpl->basePose.setPosition(iDynTree::Position(basePositionInterface.at(0), basePositionInterface.at(1), basePositionInterface.at(2))); @@ -1153,6 +1184,7 @@ void HumanWrenchProvider::run() { yError()<mapEstHelper.berdyHelper.getRangeLinkSensorVariable(iDynTree::BerdySensorTypes::NET_EXT_WRENCH_SENSOR, linkIndex); for(std::size_t j=0; jmapEstHelper.berdyOptions.baseLink); iDynTree::KinDynComputations kinDynComputations; - kinDynComputations.loadRobotModel(model); + kinDynComputations.loadRobotModel(pImpl->humanModel); kinDynComputations.setFrameVelocityRepresentation(iDynTree::FrameVelocityRepresentation::BODY_FIXED_REPRESENTATION); kinDynComputations.setRobotState(pImpl->basePose, pImpl->humanJointPositionsVec, @@ -1211,18 +1243,24 @@ void HumanWrenchProvider::run() { yError()<mapEstHelper.mapSolver->getLastEstimate(); + const iDynTree::VectorDynSize& estimatedWrenches = pImpl->mapEstHelper.mapSolver->getLastEstimate(); + + iDynTree::LinkNetExternalWrenches linkExtWrenches(pImpl->humanModel); + pImpl->mapEstHelper.berdyHelper.extractLinkNetExternalWrenchesFromDynamicVariables(estimatedWrenches,linkExtWrenches); // update transformed wrenches with estimate for (unsigned i = 0; i < pImpl->wrenchSources.size(); ++i) { + iDynTree::LinkIndex linkIndex = pImpl->mapEstHelper.berdyHelper.model().getLinkIndex(pImpl->wrenchSources[i].outputFrame); for(int j=0; j<6; j++) { - pImpl->transformedWrenches[i].setVal(j, estimatedWrenches.getVal(j)); + pImpl->transformedWrenches[i].setVal(j, linkExtWrenches(linkIndex).getVal(j)); } } + } } // Expose the data as IAnalogSensor @@ -1263,7 +1301,9 @@ bool HumanWrenchProvider::attach(yarp::dev::PolyDriver* poly) // Get the device name from the driver const std::string deviceName = poly->getValue("device").asString(); - if (deviceName == "human_state_provider") { + yInfo()<iHumanState || !poly->view(pImpl->iHumanState) || !pImpl->iHumanState) { @@ -1271,11 +1311,12 @@ bool HumanWrenchProvider::attach(yarp::dev::PolyDriver* poly) return false; } - // Check the interface - if (pImpl->iHumanState->getNumberOfJoints() == 0 + //TODO Check the interface + int count = 1000000; + while (pImpl->iHumanState->getNumberOfJoints() == 0 || pImpl->iHumanState->getNumberOfJoints() != pImpl->iHumanState->getJointNames().size()) { - yError() << "The IHumanState interface might not be ready"; - return false; + yError() << LogPrefix<<"The IHumanState interface might not be ready"; + if(count--==0) return false; } yInfo() << LogPrefix << deviceName << "attach() successful"; From 1d8e77a8e75b9ebae5c313aae6e89f82122624d5 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Fri, 8 Jul 2022 12:31:30 +0200 Subject: [PATCH 14/31] Improve iHumanState check in DynamicsEstimator --- .../HumanDynamicsEstimator/HumanDynamicsEstimator.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp b/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp index f10966867..63a592c3d 100644 --- a/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp +++ b/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp @@ -1231,11 +1231,12 @@ bool HumanDynamicsEstimator::attach(yarp::dev::PolyDriver* poly) return false; } - // Check the interface - if (pImpl->iHumanState->getNumberOfJoints() == 0 + //TODO Check the interface + int count = 1000000; + while (pImpl->iHumanState->getNumberOfJoints() == 0 || pImpl->iHumanState->getNumberOfJoints() != pImpl->iHumanState->getJointNames().size()) { - yError() << "The IHumanState interface might not be ready"; - return false; + yError() << LogPrefix<<"The IHumanState interface might not be ready"; + if(count--==0) return false; } yInfo() << LogPrefix << deviceName << "attach() successful"; From 84a6e25d6ec252b8616d5699b238c5e9616fe81c Mon Sep 17 00:00:00 2001 From: Riccardo Date: Fri, 8 Jul 2022 16:10:06 +0200 Subject: [PATCH 15/31] Add human_mass parameter in WrenchProvider --- devices/HumanWrenchProvider/HumanWrenchProvider.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index 7296e421e..15688b134 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -1259,7 +1259,7 @@ void HumanWrenchProvider::run() { pImpl->transformedWrenches[i].setVal(j, linkExtWrenches(linkIndex).getVal(j)); } - } + } } From d6623e29cb9d54ac0ac1a6f3d890e6028cd8c75c Mon Sep 17 00:00:00 2001 From: Riccardo Date: Fri, 8 Jul 2022 18:31:34 +0200 Subject: [PATCH 16/31] Add human_mass in WrenchProvider --- .../HumanWrenchProvider.cpp | 41 ++++++++++++++----- 1 file changed, 31 insertions(+), 10 deletions(-) diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index 15688b134..1920066a3 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -127,6 +127,7 @@ class HumanWrenchProvider::Impl // Human variables iDynTree::Model humanModel; + double humanMass; // buffer variables (iDynTree) iDynTree::VectorDynSize humanJointPositionsVec; iDynTree::VectorDynSize humanJointVelocitiesVec; @@ -154,6 +155,9 @@ class HumanWrenchProvider::Impl MAPEstParams mapEstParams; MAPEstHelper mapEstHelper; std::vector transformedWrenches; + + iDynTree::SpatialForceVector computeRCMInBaseUsingMeasurements(iDynTree::KinDynComputations& kinDynComputations, + iDynTree::LinkIndex baseIdx); }; HumanWrenchProvider::HumanWrenchProvider() @@ -413,24 +417,24 @@ bool parseMAPEstParams(yarp::os::Searchable& config, MAPEstParams& mapEstParams) return true; } -iDynTree::SpatialForceVector computeRCMInBaseUsingMeasurements(iDynTree::KinDynComputations& kinDynComputations, - iDynTree::LinkIndex baseIdx) +iDynTree::SpatialForceVector HumanWrenchProvider::Impl::computeRCMInBaseUsingMeasurements(iDynTree::KinDynComputations& kinDynComputations, + iDynTree::LinkIndex baseIdx) { //TODO iDynTree::SpatialForceVector rcm; rcm.zero(); //return rcm; - iDynTree::SpatialForceVector subjectWeight; - subjectWeight.zero(); - subjectWeight.setVal(2, 9.81 * kinDynComputations.model().getTotalMass()); //TODO set weight from parameter - yDebug()<humanModel = humanModelLoader.model(); pImpl->humanKinDynComp.loadRobotModel(pImpl->humanModel); + // set the human mass + pImpl->humanMass = 0.0; + if(!config.check("human_mass")) + { + yInfo() << LogPrefix << "Missing optional parameter \"human_mass\", using the model's total mass by default"; + pImpl->humanMass = pImpl->humanModel.getTotalMass(); + } + else + { + if(!(config.find("human_mass").isFloat64() || config.find("human_mass").isInt32())) + { + yError() << LogPrefix << "Parameter human_mass is not a valid number!"; + return false; + } + pImpl->humanMass = config.find("human_mass").asFloat64(); + } + // ========================== // INITIALIZE THE ROBOT MODEL // ========================== From d6b76fd1594b01b67d91bc3fed169cf7f4f0086d Mon Sep 17 00:00:00 2001 From: Riccardo Date: Tue, 12 Jul 2022 17:20:14 +0200 Subject: [PATCH 17/31] [WrenchProvider] Raise missing parameter error --- devices/HumanWrenchProvider/HumanWrenchProvider.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index 1920066a3..97c5a6229 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -277,14 +277,14 @@ bool parseMeasurementsCovariance(yarp::os::Searchable& config, MAPEstParams& map yarp::os::Bottle& priorMeasurementsCovarianceBottle = config.findGroup("cov_measurements_NET_EXT_WRENCH_SENSOR"); if(priorMeasurementsCovarianceBottle.isNull()) { - //TODO use default values? + yError() << LogPrefix << "Missing group cov_measurements_NET_EXT_WRENCH_SENSOR!"; return false; } // find the default value if(!priorMeasurementsCovarianceBottle.check("value")) { - //TODO + yError() << LogPrefix << "Missing default covariance value for NET_EXT_WRENCH_SENSORs!"; return false; } mapEstParams.measurementDefaultCovariance = priorMeasurementsCovarianceBottle.find("value").asFloat64(); From 9df0cf6bf5b14b54b112efb03d2dc1ed0f0912c9 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Tue, 12 Jul 2022 17:28:43 +0200 Subject: [PATCH 18/31] Simplify RCM calculation for the MAP estimation --- .../HumanWrenchProvider.cpp | 64 ++----------------- 1 file changed, 5 insertions(+), 59 deletions(-) diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index 97c5a6229..da985259f 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -156,8 +156,7 @@ class HumanWrenchProvider::Impl MAPEstHelper mapEstHelper; std::vector transformedWrenches; - iDynTree::SpatialForceVector computeRCMInBaseUsingMeasurements(iDynTree::KinDynComputations& kinDynComputations, - iDynTree::LinkIndex baseIdx); + iDynTree::SpatialForceVector computeRCMInBaseUsingMeasurements(iDynTree::KinDynComputations& kinDynComputations); }; HumanWrenchProvider::HumanWrenchProvider() @@ -417,16 +416,12 @@ bool parseMAPEstParams(yarp::os::Searchable& config, MAPEstParams& mapEstParams) return true; } -iDynTree::SpatialForceVector HumanWrenchProvider::Impl::computeRCMInBaseUsingMeasurements(iDynTree::KinDynComputations& kinDynComputations, - iDynTree::LinkIndex baseIdx) +iDynTree::SpatialForceVector HumanWrenchProvider::Impl::computeRCMInBaseUsingMeasurements(iDynTree::KinDynComputations& kinDynComputations) { - //TODO iDynTree::SpatialForceVector rcm; rcm.zero(); - //return rcm; iDynTree::AngularForceVector3 gravityTorque(0., 0., 0.); - iDynTree::SpatialForceVector subjectWeightInCentroidal(world_gravity, gravityTorque); subjectWeightInCentroidal = subjectWeightInCentroidal*(-humanMass); @@ -436,59 +431,10 @@ iDynTree::SpatialForceVector HumanWrenchProvider::Impl::computeRCMInBaseUsingMea iDynTree::SpatialForceVector subjectWeightInBase = base_H_world * subjectWeightInCentroidal; - return subjectWeightInBase; - - const iDynTree::Model& model = kinDynComputations.model(); - - iDynTree::LinkPositions linkPos(model); - iDynTree::LinkVelArray linkVels(model); - iDynTree::LinkAccArray linkProperAccs(model); - iDynTree::LinkInertias linkInertias(model); - - // Get links info - for(size_t linkIdx = 0; linkIdx < model.getNrOfLinks(); linkIdx++) - { - linkInertias(linkIdx) = model.getLink(linkIdx)->getInertia(); - linkPos(linkIdx) = kinDynComputations.getWorldTransform(linkIdx); - linkVels(linkIdx) = kinDynComputations.getFrameVel(linkIdx); - - //TODO manage accelerations - linkProperAccs(linkIdx).zero(); - } - - // Set centroidal to world transform - iDynTree::Transform world_H_centroidal = iDynTree::Transform(iDynTree::Rotation::Identity(), kinDynComputations.getCenterOfMassPosition()); - - // Iterate over measurements - for(iDynTree::LinkIndex visitedLinkIndex = 0; visitedLinkIndex < model.getNrOfLinks(); visitedLinkIndex++) - { - // Get world_H_link transform - const iDynTree::Transform world_H_link = linkPos(visitedLinkIndex); - - // Compute link to centroidal transform - const iDynTree::Transform centroidal_H_link = world_H_centroidal.inverse() * world_H_link; - - // Get link velocity L_v_A,L - const iDynTree::Twist linkVelocityExpressedInLink = linkVels(visitedLinkIndex); - - // Get link acceleration L_a_A,L - const iDynTree::SpatialAcc linkAccelerationExpressedInLink = linkProperAccs(visitedLinkIndex); - - // velocity contribution - const iDynTree::SpatialForceVector linkInertiaMultipliedVelocity = linkInertias(visitedLinkIndex).multiply(linkVelocityExpressedInLink); - const iDynTree::SpatialForceVector rcmLinkVelContrib = centroidal_H_link * (linkVelocityExpressedInLink.cross(linkInertiaMultipliedVelocity)); - - // acceleration contribution - const iDynTree::SpatialForceVector rcmLinkAccContrib = centroidal_H_link * (linkInertias(visitedLinkIndex).multiply(linkAccelerationExpressedInLink)); - - // update rcm - rcm = rcm + rcmLinkVelContrib + rcmLinkAccContrib; - } + rcm = rcm + subjectWeightInBase; - // base_H_centroidal transform - const iDynTree::Transform base_H_centroidal = kinDynComputations.getWorldBaseTransform().inverse() * world_H_centroidal; + return rcm; - return base_H_centroidal * rcm; } bool HumanWrenchProvider::open(yarp::os::Searchable& config) @@ -1227,7 +1173,7 @@ void HumanWrenchProvider::run() pImpl->humanJointVelocitiesVec, pImpl->world_gravity); - iDynTree::SpatialForceVector rcm = computeRCMInBaseUsingMeasurements(kinDynComputations, baseFrameIndex); + iDynTree::SpatialForceVector rcm = pImpl->computeRCMInBaseUsingMeasurements(kinDynComputations); iDynTree::IndexRange rcmSensorRange = pImpl->mapEstHelper.berdyHelper.getRangeRCMSensorVariable(iDynTree::BerdySensorTypes::RCM_SENSOR); for(std::size_t j=0; j Date: Wed, 13 Jul 2022 10:43:44 +0200 Subject: [PATCH 19/31] Set WrenchProvider base frame from params --- .../HumanWrenchProvider.cpp | 28 ++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index da985259f..2d7db279d 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -128,6 +128,8 @@ class HumanWrenchProvider::Impl // Human variables iDynTree::Model humanModel; double humanMass; + std::string baseLink; + // buffer variables (iDynTree) iDynTree::VectorDynSize humanJointPositionsVec; iDynTree::VectorDynSize humanJointVelocitiesVec; @@ -503,6 +505,30 @@ bool HumanWrenchProvider::open(yarp::os::Searchable& config) pImpl->humanMass = config.find("human_mass").asFloat64(); } + std::string baseLink = ""; + yarp::os::Value& baseLinkValue = config.find("base_name"); + if(!baseLinkValue.isString()) + { + yError() << LogPrefix << "Parameter baseLink is not a valid string!"; + return false; + } + + if(baseLinkValue.isNull()) + { + pImpl->baseLink = pImpl->humanModel.getFrameName(pImpl->humanModel.getDefaultBaseLink()); + yInfo() << LogPrefix << "Missing parameter baseLink, using the model's default base link" << pImpl->baseLink; + } + else + { + pImpl->baseLink = baseLinkValue.asString(); + if(!pImpl->humanModel.isLinkNameUsed(pImpl->baseLink)) + { + yError() << LogPrefix << "Base link name" << pImpl->baseLink << "is not valid!"; + return false; + } + pImpl->humanModel.setDefaultBaseLink(pImpl->humanModel.getFrameIndex(pImpl->baseLink)); + } + // ========================== // INITIALIZE THE ROBOT MODEL // ========================== @@ -826,7 +852,7 @@ bool HumanWrenchProvider::open(yarp::os::Searchable& config) pImpl->mapEstHelper.berdyOptions.includeAllNetExternalWrenchesAsSensors = true; pImpl->mapEstHelper.berdyOptions.includeAllNetExternalWrenchesAsDynamicVariables = true; - pImpl->mapEstHelper.berdyOptions.baseLink = "Pelvis"; //TODO use the ihumanstate one + pImpl->mapEstHelper.berdyOptions.baseLink = pImpl->baseLink; if(!pImpl->mapEstHelper.berdyOptions.checkConsistency()) { From eb19add93a6f8065d0c9ee673c4ea6795efb7810 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Thu, 14 Jul 2022 16:57:30 +0200 Subject: [PATCH 20/31] [WrenchProvider] Remove number_of_sources param --- conf/xml/Human.xml | 1 - conf/xml/hands-pHRI.xml | 1 - conf/xml/pHRI.xml | 1 - .../HumanDynamicsEstimator.cpp | 24 +++++++++---------- .../HumanWrenchProvider.cpp | 18 +++----------- 5 files changed, 15 insertions(+), 30 deletions(-) diff --git a/conf/xml/Human.xml b/conf/xml/Human.xml index 0a860d2c9..31178c53a 100644 --- a/conf/xml/Human.xml +++ b/conf/xml/Human.xml @@ -128,7 +128,6 @@ 0.100 humanSubject03_66dof.urdf false - 2 (FTShoeLeft FTShoeRight) FTShoeLeftFTSensors diff --git a/conf/xml/hands-pHRI.xml b/conf/xml/hands-pHRI.xml index 8c065ea14..58ce121f6 100644 --- a/conf/xml/hands-pHRI.xml +++ b/conf/xml/hands-pHRI.xml @@ -123,7 +123,6 @@ true model.urdf 3 - 2 (RobotLeftHandFT RobotRightHandFT) ICub::ft6D::leftWBDFTSensor diff --git a/conf/xml/pHRI.xml b/conf/xml/pHRI.xml index 4c32368c5..d08261183 100644 --- a/conf/xml/pHRI.xml +++ b/conf/xml/pHRI.xml @@ -124,7 +124,6 @@ true model.urdf 3 - 4 (FTShoeLeft FTShoeRight RobotLeftHandFT RobotRightHandFT) FTShoeLeftFTSensors diff --git a/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp b/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp index 63a592c3d..b3476cac5 100644 --- a/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp +++ b/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp @@ -1243,29 +1243,29 @@ bool HumanDynamicsEstimator::attach(yarp::dev::PolyDriver* poly) } if (deviceName == "human_wrench_provider") { - // Attach IAnalogServer interfaces coming from HumanWrenchProvider - if (pImpl->iAnalogSensor || !poly->view(pImpl->iAnalogSensor) || !pImpl->iAnalogSensor) { - yError() << LogPrefix << "Failed to view IAnalogSensor interface from the polydriver"; + // Attach IHumanWrench interfaces coming from HumanWrenchProvider + if (pImpl->iHumanWrench || !poly->view(pImpl->iHumanWrench) || !pImpl->iHumanWrench) { + yError() << LogPrefix << "Failed to view iHumanWrench interface from the polydriver"; return false; } // Check the interface - auto numberOfSensors = stoi(poly->getValue("number_of_sources").asString()); - if (pImpl->iAnalogSensor->getChannels() != 6 * numberOfSensors) { - yError() << LogPrefix << "The IAnalogSensor interface might not be ready"; + auto numberOfWrenchSources = pImpl->iHumanWrench->getNumberOfWrenchSources(); + if ( numberOfWrenchSources == 0 || + numberOfWrenchSources != pImpl->iHumanWrench->getWrenchSourceNames().size()) { + yError() << "The IHumanWrench interface might not be ready"; return false; } - // Attach IHumanWrench interfaces coming from HumanWrenchProvider - if (pImpl->iHumanWrench || !poly->view(pImpl->iHumanWrench) || !pImpl->iHumanWrench) { - yError() << LogPrefix << "Failed to view iHumanWrench interface from the polydriver"; + // Attach IAnalogServer interfaces coming from HumanWrenchProvider + if (pImpl->iAnalogSensor || !poly->view(pImpl->iAnalogSensor) || !pImpl->iAnalogSensor) { + yError() << LogPrefix << "Failed to view IAnalogSensor interface from the polydriver"; return false; } // Check the interface - if (pImpl->iHumanWrench->getNumberOfWrenchSources() == 0 - || pImpl->iHumanWrench->getNumberOfWrenchSources() != pImpl->iHumanWrench->getWrenchSourceNames().size()) { - yError() << "The IHumanWrench interface might not be ready"; + if (pImpl->iAnalogSensor->getChannels() != 6 * numberOfWrenchSources) { + yError() << LogPrefix << "The IAnalogSensor interface might not be ready"; return false; } diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index 2d7db279d..040dc7d29 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -618,11 +618,6 @@ bool HumanWrenchProvider::open(yarp::os::Searchable& config) // INITIALIZE THE WRENCH SOURCES // ============================= - if (!(config.check("number_of_sources") && config.find("number_of_sources").asInt32())) { - yError() << LogPrefix << "Option 'number_of_sources' not found or not a valid Integer"; - return false; - } - if (!(config.check("sources") && config.find("sources").isList())) { yError() << LogPrefix << "Option 'sources' not found or not a valid list"; return false; @@ -631,16 +626,9 @@ bool HumanWrenchProvider::open(yarp::os::Searchable& config) yarp::os::Bottle* listOfSources = config.find("sources").asList(); std::vector sourcesNames; - // Check the number of sources are correct - if ( config.find("number_of_sources").asInt32() != listOfSources->size()) { - yError() << LogPrefix << " 'number_of_sources' and 'sources' list provided are not matching"; - return false; - } - else { - for (unsigned i = 0; i < listOfSources->size(); ++i) { - sourcesNames.push_back(listOfSources->get(i).asString()); - yDebug() << LogPrefix << "Found source" << sourcesNames.back(); - } + for (unsigned i = 0; i < listOfSources->size(); ++i) { + sourcesNames.push_back(listOfSources->get(i).asString()); + yDebug() << LogPrefix << "Found source" << sourcesNames.back(); } // Parse the groups using the sources list names From 89400526c2d8455b82b9e2d60b1bcae4403b4627 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Mon, 18 Jul 2022 10:57:05 +0200 Subject: [PATCH 21/31] [WrenchProvider] Fix transform in RCM computation --- devices/HumanWrenchProvider/HumanWrenchProvider.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index 040dc7d29..3b59424a1 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -427,11 +427,11 @@ iDynTree::SpatialForceVector HumanWrenchProvider::Impl::computeRCMInBaseUsingMea iDynTree::SpatialForceVector subjectWeightInCentroidal(world_gravity, gravityTorque); subjectWeightInCentroidal = subjectWeightInCentroidal*(-humanMass); - iDynTree::Transform base_H_world; - base_H_world.setPosition(kinDynComputations.getCenterOfMassPosition() - kinDynComputations.getWorldBaseTransform().getPosition()); - base_H_world.setRotation(kinDynComputations.getWorldBaseTransform().getRotation().inverse()); + iDynTree::Transform base_H_centroidal; + base_H_centroidal.setPosition(kinDynComputations.getCenterOfMassPosition() - kinDynComputations.getWorldBaseTransform().getPosition()); + base_H_centroidal.setRotation(kinDynComputations.getWorldBaseTransform().getRotation().inverse()); - iDynTree::SpatialForceVector subjectWeightInBase = base_H_world * subjectWeightInCentroidal; + iDynTree::SpatialForceVector subjectWeightInBase = base_H_centroidal * subjectWeightInCentroidal; rcm = rcm + subjectWeightInBase; From 0ba611ea9e0ab257537f533ef73133b476d0edeb Mon Sep 17 00:00:00 2001 From: Riccardo Date: Mon, 18 Jul 2022 11:02:10 +0200 Subject: [PATCH 22/31] [WrenchProvider] Clean code --- .../HumanWrenchProvider.cpp | 35 +++++-------------- 1 file changed, 8 insertions(+), 27 deletions(-) diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index 3b59424a1..ee5b0c5bc 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -1175,19 +1175,7 @@ void HumanWrenchProvider::run() } // Set RCM_SENSOR measurement - iDynTree::Model &model = pImpl->mapEstHelper.berdyHelper.model(); - iDynTree::FrameIndex baseFrameIndex = model.getLinkIndex(pImpl->mapEstHelper.berdyOptions.baseLink); - - iDynTree::KinDynComputations kinDynComputations; - kinDynComputations.loadRobotModel(pImpl->humanModel); - kinDynComputations.setFrameVelocityRepresentation(iDynTree::FrameVelocityRepresentation::BODY_FIXED_REPRESENTATION); - kinDynComputations.setRobotState(pImpl->basePose, - pImpl->humanJointPositionsVec, - pImpl->baseVelocity, - pImpl->humanJointVelocitiesVec, - pImpl->world_gravity); - - iDynTree::SpatialForceVector rcm = pImpl->computeRCMInBaseUsingMeasurements(kinDynComputations); + iDynTree::SpatialForceVector rcm = pImpl->computeRCMInBaseUsingMeasurements(pImpl->humanKinDynComp); iDynTree::IndexRange rcmSensorRange = pImpl->mapEstHelper.berdyHelper.getRangeRCMSensorVariable(iDynTree::BerdySensorTypes::RCM_SENSOR); for(std::size_t j=0; jbaseVelocity.getAngularVec3(); - iDynTree::JointPosDoubleArray jointsPosition(model); - iDynTree::JointDOFsDoubleArray jointsVelocity(model); - iDynTree::JointDOFsDoubleArray jointsAcceleration(model); - + iDynTree::JointPosDoubleArray jointsPosition(pImpl->humanModel); + iDynTree::JointDOFsDoubleArray jointsVelocity(pImpl->humanModel); + iDynTree::JointDOFsDoubleArray jointsAcceleration(pImpl->humanModel); for(int i=0; ihumanJointPositionsVec.size(); i++) jointsPosition.setVal(i, pImpl->humanJointPositionsVec.getVal(i)); for(int i=0; ihumanJointVelocitiesVec.size(); i++) jointsVelocity.setVal(i, pImpl->humanJointVelocitiesVec.getVal(i)); - - // Set the kinematic information necessary for the dynamics estimation - //TODO check if needed - pImpl->mapEstHelper.berdyHelper.updateKinematicsFromFloatingBase(jointsPosition, - jointsVelocity, - baseFrameIndex, - baseAngularVelocity); - - pImpl->mapEstHelper.mapSolver->updateEstimateInformationFloatingBase(jointsPosition, jointsVelocity, - baseFrameIndex, + pImpl->humanModel.getLinkIndex(pImpl->baseLink), baseAngularVelocity, berdyMeasurementVector); + + // Compute estimate if(!pImpl->mapEstHelper.mapSolver->doEstimate()) { yError()< Date: Mon, 18 Jul 2022 11:31:32 +0200 Subject: [PATCH 23/31] Update Human.xml with MAP estimator parameters --- conf/xml/Human.xml | 40 ++++++++++++++++++---------------------- 1 file changed, 18 insertions(+), 22 deletions(-) diff --git a/conf/xml/Human.xml b/conf/xml/Human.xml index 31178c53a..3d1d5f80d 100644 --- a/conf/xml/Human.xml +++ b/conf/xml/Human.xml @@ -148,40 +148,36 @@ (-0.0026 0.0 -0.1208) - + + - true + false - 1e-6 - (LeftFoot RightFoot) - (0.0589998348 0.0589998348 0.0359999712 0.002250000225 0.002250000225 0.56e-3) - (0.0589998348 0.0589998348 0.0359999712 0.002250000225 0.002250000225 0.56e-3) + 1e-09 + (LeftHandCOM) + (1e06 1e06 1e06 1e06 1e06 1e06) + (1e-6 1e-6 1e-6 1e-6 1e-6 1e-6) + (1e-6 1e-6 1e-6 1e-6 1e-6 1e-6) + (1e-6 1e-6 1e-6 1e-6 1e-6 1e-6) + (1e-6 1e-6 1e-6 1e-6 1e-6 1e-6) + 1e-9 + 1e04 - - FTSourcesIWearRemapper From 71fc032436dd49361036a795b342b8a5b31a3835 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Tue, 19 Jul 2022 15:46:27 +0200 Subject: [PATCH 24/31] Update HDERviz with HandCOM topics --- conf/ros/rviz/HDERviz.rviz | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/conf/ros/rviz/HDERviz.rviz b/conf/ros/rviz/HDERviz.rviz index 64fe66592..9f3fe05c4 100644 --- a/conf/ros/rviz/HDERviz.rviz +++ b/conf/ros/rviz/HDERviz.rviz @@ -790,13 +790,13 @@ Visualization Manager: Arrow Width: 0.30000001192092896 Class: rviz/WrenchStamped Enabled: true - Force Arrow Scale: 0.02500000037252903 - Force Color: 0; 255; 200 - Hide Small Values: true + Force Arrow Scale: 0.019999999552965164 + Force Color: 255; 239; 6 + Hide Small Values: false History Length: 1 Name: LeftHand_ExtWrenches - Topic: /HDE/WrenchStamped/LeftHand - Torque Arrow Scale: 0 + Topic: /HDE/WrenchStamped/LeftHandCOM + Torque Arrow Scale: 0.5 Torque Color: 37; 95; 255 Unreliable: false Value: true @@ -804,13 +804,13 @@ Visualization Manager: Arrow Width: 0.30000001192092896 Class: rviz/WrenchStamped Enabled: true - Force Arrow Scale: 0.02500000037252903 - Force Color: 0; 255; 200 - Hide Small Values: true + Force Arrow Scale: 0.019999999552965164 + Force Color: 255; 239; 6 + Hide Small Values: false History Length: 1 Name: RightHand_ExtWrenches - Topic: /HDE/WrenchStamped/RightHand - Torque Arrow Scale: 0 + Topic: /HDE/WrenchStamped/RightHandCOM + Torque Arrow Scale: 0.5 Torque Color: 37; 95; 255 Unreliable: false Value: true From e433ce800de53788ef3f0d9acb6b835e804d777c Mon Sep 17 00:00:00 2001 From: Riccardo Date: Thu, 21 Jul 2022 19:37:37 +0200 Subject: [PATCH 25/31] Add HumanIFeel.xml --- conf/xml/HumanIFeel.xml | 409 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 409 insertions(+) create mode 100644 conf/xml/HumanIFeel.xml diff --git a/conf/xml/HumanIFeel.xml b/conf/xml/HumanIFeel.xml new file mode 100644 index 000000000..23046dd5e --- /dev/null +++ b/conf/xml/HumanIFeel.xml @@ -0,0 +1,409 @@ + + + + + + + 0.2 + + true + false + + + + + (/iFeelSuit/WearableData/data:o) + + + + 0.0167 + humanSubject03_48dof.urdf + Pelvis + + dynamical + true + false + + 300 + ma27 + 2 + 0.0 + 1.0 + 0.000001 + 0.001 + + + QP + 1.0 + 1.0 + + 10.0 + (1.0 1.0) + (60.0 20.0) + (0.0 0.0) + + + (Pelvis, iFeelSuit::vLink::Node#3, orientationAndVelocity) + (T8, iFeelSuit::vLink::Node#6, orientationAndVelocity) + (RightUpperArm, iFeelSuit::vLink::Node#7, orientationAndVelocity) + (RightForeArm, iFeelSuit::vLink::Node#8, orientationAndVelocity) + (LeftUpperArm, iFeelSuit::vLink::Node#5, orientationAndVelocity) + (LeftForeArm, iFeelSuit::vLink::Node#4, orientationAndVelocity) + (RightUpperLeg, iFeelSuit::vLink::Node#11, orientationAndVelocity) + (RightLowerLeg, iFeelSuit::vLink::Node#12, orientationAndVelocity) + (RightFoot, iFeelSuit::vLink::Node#2, orientationAndVelocity) + (LeftUpperLeg, iFeelSuit::vLink::Node#9, orientationAndVelocity) + (LeftLowerLeg, iFeelSuit::vLink::Node#10, orientationAndVelocity) + (LeftFoot, iFeelSuit::vLink::Node#1, orientationAndVelocity) + (RightToe, iFeelSuit::ft6D::Node#2, floorContact) + (LeftToe, iFeelSuit::ft6D::Node#1, floorContact) + + + 60.0 + 60.0 + + ("jT9T8_rotx", + "jT9T8_rotz", + "jRightShoulder_rotx", + "jRightShoulder_roty", + "jRightShoulder_rotz", + "jRightElbow_roty", + "jRightElbow_rotz", + "jLeftShoulder_rotx", + "jLeftShoulder_roty", + "jLeftShoulder_rotz", + "jLeftElbow_roty", + "jLeftElbow_rotz", + "jLeftHip_rotx", + "jLeftHip_roty", + "jLeftHip_rotz", + "jLeftKnee_roty", + "jLeftKnee_rotz" + "jLeftAnkle_rotx", + "jLeftAnkle_roty", + "jLeftAnkle_rotz", + "jLeftBallFoot_roty", + "jRightHip_rotx", + "jRightHip_roty", + "jRightHip_rotz", + "jRightKnee_roty", + "jRightKnee_rotz", + "jRightAnkle_rotx", + "jRightAnkle_roty", + "jRightAnkle_rotz", + "jRightBallFoot_roty", + "jL5S1_roty") + + + ( 1.0 0.0 0.0 0.0 + 0.0 0.0 -1.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 0.0 1.0) + ( 1.0 0.0 0.0 0.0 + 0.0 0.0 -1.0 0.0 + 0.0 1.0 0.0 0.0 + 0.0 0.0 0.0 1.0) + ( 1.0 0.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + 0.0 -1.0 0.0 0.0 + 0.0 0.0 0.0 1.0) + ( 1.0 0.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + 0.0 -1.0 0.0 0.0 + 0.0 0.0 0.0 1.0) + ( 0.0 1.0 0.0 0.0 + 0.0 0.0 -1.0 0.0 + -1.0 0.0 0.0 0.0 + 0.0 0.0 0.0 1.0) + ( 0.0 1.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + 1.0 0.0 0.0 0.0 + 0.0 0.0 0.0 1.0) + ( 0.0 1.0 0.0 0.0 + -1.0 0.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + 0.0 0.0 0.0 1.0) + ( 0.0 1.0 0.0 0.0 + -1.0 0.0 0.0 0.0 + 0.0 0.0 1.0 0.0 + 0.0 0.0 0.0 1.0) + + + + + + + () + + + () + + (100.0, 100.0, 100.0 , 100.0, 100.0, 100.0 ) + (-100.0, -100.0, -100.0, -100.0, -100.0, -100.0 ) + + + (jLeftKnee_rotz, jRightKnee_rotz, jLeftHip_rotx, jRightHip_rotx) + ( + ( 1.0, 0.0, 0.0, 0.0), + (-1.0, 0.0, 0.0, 0.0), + ( 0.0, 1.0, 0.0, 0.0), + ( 0.0, -1.0, 0.0, 0.0), + ( 0.0, 0.0, 1.0, 0.0), + ( 0.0, 0.0, 0.0, 1.0)) + (0.0, 0.0, 0.0, 0.0, 0.0, -100.0) + (0.0, 0.0, 0.0, 0.0, 100.0, 0.0) + + 0.5 + 0.5 + + + + IWearRemapper + + + + + + + 0.02 + /HDE/HumanStateWrapper/state:o + + + HumanStateProvider + + + + + + + 0.01 + /HDE/WearableTargetsWrapper/state:o + + + HumanStateProvider + + + + + + + 0.02 + /Human/Pelvis + /Human/joint_states + (0.0 0.0 0.0) + (0.0 0.0 0.0 0.0) + + + HumanStateProvider + + + + + + + 0.020 + humanSubject03_48dof.urdf + 70.0 + Pelvis + false + 4 + (iFeelSuit::ft6D::Node#1 iFeelSuit::ft6D::Node#2 LeftHandCOM RightHandCOM) + + iFeelSuit::ft6D::Node#1 + LeftFoot + fixed + (1.0 0.0 0.0 + 0.0 1.0 0.0 + 0.0 0.0 1.0) + (0.0 0.0 0.0) + + + iFeelSuit::ft6D::Node#2 + RightFoot + fixed + (1.0 0.0 0.0 + 0.0 1.0 0.0 + 0.0 0.0 1.0) + (0.0 0.0 0.0) + + + none + LeftHandCOM + dummy + (0.0 0.0 0.0 0.0 0.0 0.0) + + + none + RightHandCOM + dummy + (0.0 0.0 0.0 0.0 0.0 0.0) + + + + + true + + 1e-09 + (LeftFoot RightFoot LeftHandCOM RightHandCOM) + (1e06 1e06 1e06 1e-06 1e-06 1e-06) + (1e06 1e06 1e06 1e-06 1e-06 1e-06) + (1e03 1e03 1e-06 1e03 1e03 1e03) + (1e03 1e03 1e-06 1e03 1e03 1e03) + + (1e-6 1e-6 1e-6 1e03 1e03 1e03) + 1e-9 + 1e01 + + + + HumanStateProvider + IWearRemapper + + + + + + + /HDE/HumanWrenchWrapper/wrench:o + 10 + + + HumanWrenchProvider + + + + + + + /HDE/HumanWrenchPublisher/wrench:o + 20 + 24 + + 0 5 0 5 + 6 11 0 5 + 12 17 0 5 + 18 23 0 5 + + + true + (/HDE/WrenchStamped/LeftFoot /HDE/WrenchStamped/RightFoot /HDE/WrenchStamped/LeftHandCOM /HDE/WrenchStamped/RightHandCOM) + /HDE/HumanWrenchPublisherNode + geometry_msgs/WrenchStamped + (Human/LeftFoot Human/RightFoot Human/LeftHandCOM Human/RightHandCOM) + + + + HumanWrenchProvider + + + + + + + + 0.100 + humanSubject03_48dof.urdf + Pelvis + 4 + (LeftFoot RightFoot LeftHandCOM RightHandCOM) + + 0.0 + 1.0 + 0.0001 + (0.0011 0.0011 0.0011) + (0.00011 0.00011 0.00011) + 0.666e-5 + + 1e-6 + () + (0.0589998348 0.0589998348 0.0359999712 0.002250000225 0.002250000225 0.56e-3) + (0.0589998348 0.0589998348 0.0359999712 0.002250000225 0.002250000225 0.56e-3) + + + ("jT9T8_rotx", + "jT9T8_rotz", + "jRightShoulder_rotx", + "jRightShoulder_roty", + "jRightShoulder_rotz", + "jRightElbow_roty", + "jRightElbow_rotz", + "jLeftShoulder_rotx", + "jLeftShoulder_roty", + "jLeftShoulder_rotz", + "jLeftElbow_roty", + "jLeftElbow_rotz", + "jLeftHip_rotx", + "jLeftHip_roty", + "jLeftHip_rotz", + "jLeftKnee_roty", + "jLeftKnee_rotz" + "jLeftAnkle_rotx", + "jLeftAnkle_roty", + "jLeftAnkle_rotz", + "jLeftBallFoot_roty", + "jRightHip_rotx", + "jRightHip_roty", + "jRightHip_rotz", + "jRightKnee_roty", + "jRightKnee_rotz", + "jRightAnkle_rotx", + "jRightAnkle_roty", + "jRightAnkle_rotz", + "jRightBallFoot_roty", + "jL5S1_roty", + "jRightWrist_rotz", + "jRightWrist_rotx", + "jLeftWrist_rotz", + "jLeftWrist_rotx", + "jRightHandCOM", + "jLeftHandCOM") + + + * + * + + + + HumanStateProvider + HumanWrenchProvider + + + + + + + 0.1 + /HDE/HumanDynamicsWrapper/torques:o + + + HumanDynamicsEstimator + + + + + + + 0.020 + (LeftFoot LeftLowerLeg LeftUpperLeg LeftHand LeftForeArm LeftUpperArm RightFoot RightLowerLeg RightUpperLeg RightHand RightForeArm RightUpperArm L5) + (jLeftAnkle jLeftKnee jLeftHip jLeftWrist jLeftElbow jLeftShoulder jRightAnkle jRightKnee jRightHip jRightWrist jRightElbow jRightShoulder jL5S1) + /HumanEffortBridge + Human + + + HumanDynamicsEstimator + + + + + + From 6d9525d3e62307f6ac01f445367afedd013089cf Mon Sep 17 00:00:00 2001 From: Riccardo Date: Thu, 21 Jul 2022 19:39:16 +0200 Subject: [PATCH 26/31] Use iHumanState jointNames to populate the buffers --- .../HumanDynamicsEstimator.cpp | 20 +++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp b/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp index b3476cac5..6fa80ed16 100644 --- a/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp +++ b/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp @@ -1087,6 +1087,7 @@ void HumanDynamicsEstimator::run() // Get state data from the attached IHumanState interface std::vector jointsPosition = pImpl->iHumanState->getJointPositions(); std::vector jointsVelocity = pImpl->iHumanState->getJointVelocities(); + std::vector jointNames = pImpl->iHumanState->getJointNames(); std::array basePosition = pImpl->iHumanState->getBasePosition(); std::array baseOrientation = pImpl->iHumanState->getBaseOrientation(); @@ -1098,14 +1099,17 @@ void HumanDynamicsEstimator::run() pImpl->berdyData.state.baseAngularVelocity.setVal(2, baseVelocity.at(5)); // Set the received state data to berdy state variables - pImpl->berdyData.state.jointsPosition.resize(jointsPosition.size()); - for (size_t i = 0; i < jointsPosition.size(); i++) { - pImpl->berdyData.state.jointsPosition.setVal(i, jointsPosition.at(i)); - } - - pImpl->berdyData.state.jointsVelocity.resize(jointsVelocity.size()); - for (size_t i = 0; i < jointsVelocity.size(); ++i) { - pImpl->berdyData.state.jointsVelocity.setVal(i, jointsVelocity.at(i)); + for(int i=0; ihumanModel.getJointIndex(jointNames[i]); + if(jointIndex==iDynTree::JOINT_INVALID_INDEX) + { + yError() << LogPrefix << "Joint"<berdyData.state.jointsPosition.setVal(jointIndex, jointsPosition[i]); + pImpl->berdyData.state.jointsVelocity.setVal(jointIndex, jointsVelocity[i]); } // Fill in the y vector with sensor measurements for the FT sensors From 550efacdec32233fe7f970b14757dcfc20d68211 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Thu, 21 Jul 2022 19:45:48 +0200 Subject: [PATCH 27/31] Update HDERviz.rviz --- conf/ros/rviz/HDERviz.rviz | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/conf/ros/rviz/HDERviz.rviz b/conf/ros/rviz/HDERviz.rviz index 9f3fe05c4..51b3186c6 100644 --- a/conf/ros/rviz/HDERviz.rviz +++ b/conf/ros/rviz/HDERviz.rviz @@ -383,7 +383,7 @@ Visualization Manager: Enabled: true Invert Rainbow: true Max Color: 239; 41; 41 - Max Intensity: 20 + Max Intensity: 30 Min Color: 138; 226; 52 Min Intensity: 0 Name: jL5S1Effort @@ -533,7 +533,7 @@ Visualization Manager: Enabled: true Invert Rainbow: true Max Color: 239; 41; 41 - Max Intensity: 40 + Max Intensity: 30 Min Color: 138; 226; 52 Min Intensity: 0 Name: jLeftShoulderEffort @@ -563,7 +563,7 @@ Visualization Manager: Enabled: true Invert Rainbow: true Max Color: 239; 41; 41 - Max Intensity: 15 + Max Intensity: 6 Min Color: 138; 226; 52 Min Intensity: 0 Name: jLeftWristEffort @@ -713,7 +713,7 @@ Visualization Manager: Enabled: true Invert Rainbow: true Max Color: 239; 41; 41 - Max Intensity: 40 + Max Intensity: 30 Min Color: 138; 226; 52 Min Intensity: 0 Name: jRightShoulderEffort @@ -743,7 +743,7 @@ Visualization Manager: Enabled: true Invert Rainbow: true Max Color: 239; 41; 41 - Max Intensity: 15 + Max Intensity: 6 Min Color: 138; 226; 52 Min Intensity: 0 Name: jRightWristEffort From 772a12dbfc084062981e29b90188cae05f9580f9 Mon Sep 17 00:00:00 2001 From: Riccardo Date: Mon, 25 Jul 2022 17:21:38 +0200 Subject: [PATCH 28/31] Fix HumanWrenchProvider includes --- devices/HumanWrenchProvider/HumanWrenchProvider.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index ee5b0c5bc..ae235961d 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include #include From 19e2a86d41dbb1d18d6f89b3309ed2868c7197d4 Mon Sep 17 00:00:00 2001 From: Riccardo Grieco Date: Thu, 25 Aug 2022 14:53:17 +0200 Subject: [PATCH 29/31] Improve HumanWrenchProvider readability Co-authored-by: Silvio Traversaro --- devices/HumanWrenchProvider/HumanWrenchProvider.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index ae235961d..8043d16a0 100644 --- a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp +++ b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp @@ -1279,7 +1279,8 @@ bool HumanWrenchProvider::attach(yarp::dev::PolyDriver* poly) while (pImpl->iHumanState->getNumberOfJoints() == 0 || pImpl->iHumanState->getNumberOfJoints() != pImpl->iHumanState->getJointNames().size()) { yError() << LogPrefix<<"The IHumanState interface might not be ready"; - if(count--==0) return false; + count--; + if(count==0) return false; } yInfo() << LogPrefix << deviceName << "attach() successful"; From cd9f6cd60f285b0ddfa342f9db5a02b1a19ac5dc Mon Sep 17 00:00:00 2001 From: Riccardo Date: Thu, 1 Sep 2022 16:26:20 +0200 Subject: [PATCH 30/31] Use timeout for checking iHumanState interface --- .../HumanDynamicsEstimator.cpp | 14 +++++++++++--- .../HumanWrenchProvider/HumanWrenchProvider.cpp | 16 ++++++++++++---- 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp b/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp index 6fa80ed16..9598ef398 100644 --- a/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp +++ b/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp @@ -43,6 +43,9 @@ const std::string DeviceName = "HumanDynamicsEstimator"; const std::string LogPrefix = DeviceName + " :"; constexpr double DefaultPeriod = 0.01; +// Timeout for checking that the iHumanState interface is providing consistent data +constexpr static double INTERFACE_CHECK_TIMEOUT_S = 10; + using namespace hde::devices; static bool parseYarpValueToStdVector(const yarp::os::Value& option, std::vector& output) @@ -1235,12 +1238,17 @@ bool HumanDynamicsEstimator::attach(yarp::dev::PolyDriver* poly) return false; } - //TODO Check the interface + // Check the iHumanState interface + double interfaceCheckStart = yarp::os::Time::now(); int count = 1000000; while (pImpl->iHumanState->getNumberOfJoints() == 0 || pImpl->iHumanState->getNumberOfJoints() != pImpl->iHumanState->getJointNames().size()) { - yError() << LogPrefix<<"The IHumanState interface might not be ready"; - if(count--==0) return false; + + if(yarp::os::Time::now()-interfaceCheckStart>INTERFACE_CHECK_TIMEOUT_S) + { + yError() << LogPrefix<<"The iHumanState interface has been providing inconsistent data for"<iHumanState->getNumberOfJoints() == 0 || pImpl->iHumanState->getNumberOfJoints() != pImpl->iHumanState->getJointNames().size()) { - yError() << LogPrefix<<"The IHumanState interface might not be ready"; - count--; - if(count==0) return false; + + if(yarp::os::Time::now()-interfaceCheckStart>INTERFACE_CHECK_TIMEOUT_S) + { + yError() << LogPrefix<<"The iHumanState interface has been providing inconsistent data for"< Date: Thu, 1 Sep 2022 16:49:38 +0200 Subject: [PATCH 31/31] Remove unused configuration file --- conf/xml/HumanIFeel.xml | 409 ---------------------------------------- 1 file changed, 409 deletions(-) delete mode 100644 conf/xml/HumanIFeel.xml diff --git a/conf/xml/HumanIFeel.xml b/conf/xml/HumanIFeel.xml deleted file mode 100644 index 23046dd5e..000000000 --- a/conf/xml/HumanIFeel.xml +++ /dev/null @@ -1,409 +0,0 @@ - - - - - - - 0.2 - - true - false - - - - - (/iFeelSuit/WearableData/data:o) - - - - 0.0167 - humanSubject03_48dof.urdf - Pelvis - - dynamical - true - false - - 300 - ma27 - 2 - 0.0 - 1.0 - 0.000001 - 0.001 - - - QP - 1.0 - 1.0 - - 10.0 - (1.0 1.0) - (60.0 20.0) - (0.0 0.0) - - - (Pelvis, iFeelSuit::vLink::Node#3, orientationAndVelocity) - (T8, iFeelSuit::vLink::Node#6, orientationAndVelocity) - (RightUpperArm, iFeelSuit::vLink::Node#7, orientationAndVelocity) - (RightForeArm, iFeelSuit::vLink::Node#8, orientationAndVelocity) - (LeftUpperArm, iFeelSuit::vLink::Node#5, orientationAndVelocity) - (LeftForeArm, iFeelSuit::vLink::Node#4, orientationAndVelocity) - (RightUpperLeg, iFeelSuit::vLink::Node#11, orientationAndVelocity) - (RightLowerLeg, iFeelSuit::vLink::Node#12, orientationAndVelocity) - (RightFoot, iFeelSuit::vLink::Node#2, orientationAndVelocity) - (LeftUpperLeg, iFeelSuit::vLink::Node#9, orientationAndVelocity) - (LeftLowerLeg, iFeelSuit::vLink::Node#10, orientationAndVelocity) - (LeftFoot, iFeelSuit::vLink::Node#1, orientationAndVelocity) - (RightToe, iFeelSuit::ft6D::Node#2, floorContact) - (LeftToe, iFeelSuit::ft6D::Node#1, floorContact) - - - 60.0 - 60.0 - - ("jT9T8_rotx", - "jT9T8_rotz", - "jRightShoulder_rotx", - "jRightShoulder_roty", - "jRightShoulder_rotz", - "jRightElbow_roty", - "jRightElbow_rotz", - "jLeftShoulder_rotx", - "jLeftShoulder_roty", - "jLeftShoulder_rotz", - "jLeftElbow_roty", - "jLeftElbow_rotz", - "jLeftHip_rotx", - "jLeftHip_roty", - "jLeftHip_rotz", - "jLeftKnee_roty", - "jLeftKnee_rotz" - "jLeftAnkle_rotx", - "jLeftAnkle_roty", - "jLeftAnkle_rotz", - "jLeftBallFoot_roty", - "jRightHip_rotx", - "jRightHip_roty", - "jRightHip_rotz", - "jRightKnee_roty", - "jRightKnee_rotz", - "jRightAnkle_rotx", - "jRightAnkle_roty", - "jRightAnkle_rotz", - "jRightBallFoot_roty", - "jL5S1_roty") - - - ( 1.0 0.0 0.0 0.0 - 0.0 0.0 -1.0 0.0 - 0.0 1.0 0.0 0.0 - 0.0 0.0 0.0 1.0) - ( 1.0 0.0 0.0 0.0 - 0.0 0.0 -1.0 0.0 - 0.0 1.0 0.0 0.0 - 0.0 0.0 0.0 1.0) - ( 1.0 0.0 0.0 0.0 - 0.0 0.0 1.0 0.0 - 0.0 -1.0 0.0 0.0 - 0.0 0.0 0.0 1.0) - ( 1.0 0.0 0.0 0.0 - 0.0 0.0 1.0 0.0 - 0.0 -1.0 0.0 0.0 - 0.0 0.0 0.0 1.0) - ( 0.0 1.0 0.0 0.0 - 0.0 0.0 -1.0 0.0 - -1.0 0.0 0.0 0.0 - 0.0 0.0 0.0 1.0) - ( 0.0 1.0 0.0 0.0 - 0.0 0.0 1.0 0.0 - 1.0 0.0 0.0 0.0 - 0.0 0.0 0.0 1.0) - ( 0.0 1.0 0.0 0.0 - -1.0 0.0 0.0 0.0 - 0.0 0.0 1.0 0.0 - 0.0 0.0 0.0 1.0) - ( 0.0 1.0 0.0 0.0 - -1.0 0.0 0.0 0.0 - 0.0 0.0 1.0 0.0 - 0.0 0.0 0.0 1.0) - - - - - - - () - - - () - - (100.0, 100.0, 100.0 , 100.0, 100.0, 100.0 ) - (-100.0, -100.0, -100.0, -100.0, -100.0, -100.0 ) - - - (jLeftKnee_rotz, jRightKnee_rotz, jLeftHip_rotx, jRightHip_rotx) - ( - ( 1.0, 0.0, 0.0, 0.0), - (-1.0, 0.0, 0.0, 0.0), - ( 0.0, 1.0, 0.0, 0.0), - ( 0.0, -1.0, 0.0, 0.0), - ( 0.0, 0.0, 1.0, 0.0), - ( 0.0, 0.0, 0.0, 1.0)) - (0.0, 0.0, 0.0, 0.0, 0.0, -100.0) - (0.0, 0.0, 0.0, 0.0, 100.0, 0.0) - - 0.5 - 0.5 - - - - IWearRemapper - - - - - - - 0.02 - /HDE/HumanStateWrapper/state:o - - - HumanStateProvider - - - - - - - 0.01 - /HDE/WearableTargetsWrapper/state:o - - - HumanStateProvider - - - - - - - 0.02 - /Human/Pelvis - /Human/joint_states - (0.0 0.0 0.0) - (0.0 0.0 0.0 0.0) - - - HumanStateProvider - - - - - - - 0.020 - humanSubject03_48dof.urdf - 70.0 - Pelvis - false - 4 - (iFeelSuit::ft6D::Node#1 iFeelSuit::ft6D::Node#2 LeftHandCOM RightHandCOM) - - iFeelSuit::ft6D::Node#1 - LeftFoot - fixed - (1.0 0.0 0.0 - 0.0 1.0 0.0 - 0.0 0.0 1.0) - (0.0 0.0 0.0) - - - iFeelSuit::ft6D::Node#2 - RightFoot - fixed - (1.0 0.0 0.0 - 0.0 1.0 0.0 - 0.0 0.0 1.0) - (0.0 0.0 0.0) - - - none - LeftHandCOM - dummy - (0.0 0.0 0.0 0.0 0.0 0.0) - - - none - RightHandCOM - dummy - (0.0 0.0 0.0 0.0 0.0 0.0) - - - - - true - - 1e-09 - (LeftFoot RightFoot LeftHandCOM RightHandCOM) - (1e06 1e06 1e06 1e-06 1e-06 1e-06) - (1e06 1e06 1e06 1e-06 1e-06 1e-06) - (1e03 1e03 1e-06 1e03 1e03 1e03) - (1e03 1e03 1e-06 1e03 1e03 1e03) - - (1e-6 1e-6 1e-6 1e03 1e03 1e03) - 1e-9 - 1e01 - - - - HumanStateProvider - IWearRemapper - - - - - - - /HDE/HumanWrenchWrapper/wrench:o - 10 - - - HumanWrenchProvider - - - - - - - /HDE/HumanWrenchPublisher/wrench:o - 20 - 24 - - 0 5 0 5 - 6 11 0 5 - 12 17 0 5 - 18 23 0 5 - - - true - (/HDE/WrenchStamped/LeftFoot /HDE/WrenchStamped/RightFoot /HDE/WrenchStamped/LeftHandCOM /HDE/WrenchStamped/RightHandCOM) - /HDE/HumanWrenchPublisherNode - geometry_msgs/WrenchStamped - (Human/LeftFoot Human/RightFoot Human/LeftHandCOM Human/RightHandCOM) - - - - HumanWrenchProvider - - - - - - - - 0.100 - humanSubject03_48dof.urdf - Pelvis - 4 - (LeftFoot RightFoot LeftHandCOM RightHandCOM) - - 0.0 - 1.0 - 0.0001 - (0.0011 0.0011 0.0011) - (0.00011 0.00011 0.00011) - 0.666e-5 - - 1e-6 - () - (0.0589998348 0.0589998348 0.0359999712 0.002250000225 0.002250000225 0.56e-3) - (0.0589998348 0.0589998348 0.0359999712 0.002250000225 0.002250000225 0.56e-3) - - - ("jT9T8_rotx", - "jT9T8_rotz", - "jRightShoulder_rotx", - "jRightShoulder_roty", - "jRightShoulder_rotz", - "jRightElbow_roty", - "jRightElbow_rotz", - "jLeftShoulder_rotx", - "jLeftShoulder_roty", - "jLeftShoulder_rotz", - "jLeftElbow_roty", - "jLeftElbow_rotz", - "jLeftHip_rotx", - "jLeftHip_roty", - "jLeftHip_rotz", - "jLeftKnee_roty", - "jLeftKnee_rotz" - "jLeftAnkle_rotx", - "jLeftAnkle_roty", - "jLeftAnkle_rotz", - "jLeftBallFoot_roty", - "jRightHip_rotx", - "jRightHip_roty", - "jRightHip_rotz", - "jRightKnee_roty", - "jRightKnee_rotz", - "jRightAnkle_rotx", - "jRightAnkle_roty", - "jRightAnkle_rotz", - "jRightBallFoot_roty", - "jL5S1_roty", - "jRightWrist_rotz", - "jRightWrist_rotx", - "jLeftWrist_rotz", - "jLeftWrist_rotx", - "jRightHandCOM", - "jLeftHandCOM") - - - * - * - - - - HumanStateProvider - HumanWrenchProvider - - - - - - - 0.1 - /HDE/HumanDynamicsWrapper/torques:o - - - HumanDynamicsEstimator - - - - - - - 0.020 - (LeftFoot LeftLowerLeg LeftUpperLeg LeftHand LeftForeArm LeftUpperArm RightFoot RightLowerLeg RightUpperLeg RightHand RightForeArm RightUpperArm L5) - (jLeftAnkle jLeftKnee jLeftHip jLeftWrist jLeftElbow jLeftShoulder jRightAnkle jRightKnee jRightHip jRightWrist jRightElbow jRightShoulder jL5S1) - /HumanEffortBridge - Human - - - HumanDynamicsEstimator - - - - - -